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ABSTRACT 


The work in this thesis is in support of a larger research effort to implement a 
cluster of autonomous airborne vehicles with the capability to conduct coordinated flight 
maneuver planning and to perform distributed sensor fusion. Specifically, it seeks to 
design and implement an onboard flight control and navigation system for NFS FROG 
UAV, which will be used as the autonomous airborne vehicle for the research, using the 
newly marketed xPC Target Rapid Prototyping System from The Mathworks, Inc. Part I 
briefly introduces the aircraft and explains the necessity for an onboard computer for the 
UAV. Part II describes the construction of the miniature aircraft computer, INS/GPS and 
air data sensor integration implementation as well as the rapid prototyping process. Part 
III covers the process to create a 6DOF model for the aircraft and the design of the 
aircraft autopilot, while Part IV presents a vision-based navigation algorithm that can be 
implemented on the UAV to give it some form of autonomous flight trajectory planning 
capability. Preliminary ground test results are presented in Part V to conclude this study. 
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I. INTRODUCTION 


A. BACKGROUND 

The Navy envisions that the battlefield of the future would inelude elusters of 
autonomous mobile agents equipped with a large number of sensors eonneeted by 
wireless networks in a hostile and highly dynamie eombat environment suseeptible to 
hardware failures and jamming. As a result, ehanges in network topology and loss of 
eonneetivity between agents are expeeted. To allow the autonomous agents to eontinue 
their missions despite these ehanges in the network quality of service (QoS), they must 
have adequate ability to integrate data from as many working onboard sensors as possible 
to assess accurately the situational picture for decision making and for executing 
appropriate flight maneuvers. 

The research question in support of such a battlefield setup is the execution of 
command, control and autonomous intelligent flight maneuver planning of a group of 
unmanned aerial vehicles and the construction of a distributed adaptive architecture for 
fusing sensor data over dynamically varying wireless networks [1]. The main thrust of 
this thesis is in line with the first research area mentioned above, i.e., to develop the basic 
framework to implement command and control (C ) of a friendly cluster of autonomous 
unmanned aerial vehicles (UAVs) including algorithms for flight navigation and 
trajectory tracking in order to adopt certain flight profiles at various stages of the mission. 


B. OBJECTIVES 

This thesis seeks to implement an autonomous flight control and target approach 
guidance algorithm using the NFS FROG UAV as a test platform. The scope of work 
includes designing the autopilot for the aircraft, exploring suitable trajectory planning 
navigation algorithms and assembling an onboard computer to perform data fusion, flight 
control and guidance commands computation. 

In addition, the timing of this effort coincided with the emergence of the xPC 
Target Rapid Prototyping System from The Mathworks, Inc. Such a system offers 
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enormous flexibility for implementing variants of the guidanee and eontrol algorithm or 
ehanges to the hardware arehiteeture with signifieantly eompressed design-to-flight-test 
time. Henee, a seeondary objeetive of this projeet is to explore and aeeumulate expertise 
on this new tool, and apply it to the intended setup. 

Details of both of these objeetives will be eovered in subsequent ehapters. The 
rest of this ehapter will serve as a lead-in by introdueing the FROG UAV, whieh will aet 
as the test aireraft, its instrumentation and the baekground on why an onboard eomputer 
is deemed neeessary to implement the intended researeh objeetive. 


C. THE AIRCRAFT 

NFS’s FROG UAV, shown in Figure LI, has been the test bed for advaneed 
eontrol and airborne sensor projeets at the Naval Postgraduate Sehool [2,3]. It is 
manufaetured by BAI Aerosystems as the BAI-TERN (Taetieally Expendable Remote 
Navigator) and derives from the EOG-R variant of the BAI-TERN used by the US Army, 
henee the name ‘EROG’. It is a small high wing monoplane with eonventional elevator, 
rudder, ailerons and flaps, and uses servomotors designed for radio-eontrolled airplanes 
to drive the eontrol surfaees. (Eigure 1.2). More details on its physieal charaeteristies and 
engine are doeumented in Appendix A and in [4]. 

Previous eontrol system projeets made use of only very basie inertial sensing and 
a simple eleetromeehanieal autopilot in the aireraft. In the existing setup, the eomputer 
(shown in Figure 1.3) whieh monitors flight data and eomputes aireraft eontrol eommands 
is loeated on the ground. Henee, raw flight data has to be downlinked from the aireraft to 
the eomputer via wireless serial modems for proeessing. Computed eontrol inputs in turn 
are pulse-eode modulated and re-transmitted baek to the aireraft using a hand-held 
Futaba® remote transmitter, shown in Figure 1.4, for the Futaba® reeeiver in the aireraft to 
interpret and output PWM eommands to drive servos that eontrols the eontrol surfaees. 
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Figure 1.1 NFS FROG UAV 



Figure 1.2 FROG UAV 3 View Drawing. 
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Figure 1.3 AC-104 Ground Control Computer 



Figure 1.4 Futaba® Transmitter, Receiver and Servos 
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D, 


CONTROL SETUP & ITS LIMITATIONS 


The original setup introdueed above imposes signifieant limitations on the 
eomplexity of the flight eontroller beeause of lateney time to control commands 
generated by the computer on the ground. It also severely restricts the flight profiles that 
can be experimented due to operating range. An alternative method of controlling the 
servos to shorten the delay between computer command output and servo actuation was 
explored in [5]. It too could not fully meet desired specifications. The limitations are 
explained below. 

1. Old Control Setup 

In the original control scheme for the UAV illustrated in Figure 1.5, the flight 
control computer was an AC-104 computer situated on the ground. Command signals 
from the AC-104 computer had to be converted to a pulse-code modulated (PCM) signal 
by a Futaba® radio controlled transmitter, which broadcasts them to the airplane. The 
Futaba® receiver in the FROG UAV decodes the PCM signal and generates pulse-width- 
modulated (PWM) commands for each of the control servos to control the aircraft. At the 
same time, in the feedback channel, sensor outputs are captured by auxiliary 
microprocessors, digitized and transmitted via wireless modem to the flight control 
computer on the ground for processing. Such a setup imposed severe controller 
restrictions due to the latency times for data downlink, control inputs computation on 
ground and the command uplink. In fact, the command uplink latency alone was 
measured to be approximately 170 ms in [4] and was found to generate unacceptable 
delay for any practical control frequency in the range of 20Hz to 40Hz. 
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Figure 1.5 Original Flight Control Setup (From [4]) 


2, Alternative Servo Command 

The short range of the Futaba® remote controller and command path latency led to 
a feasibility study of using an alternate command uplink method by CDR Chris Flood and 
the author in [5]. In the proposed scheme, control commands were sent via serial modem 
from the AC-104 directly to a small onboard micro-controller which generates the PWM 
commands to the servo instead of routing the commands through the Futaba® remote 
control. Such a setup reduced the command latency to 76 msec. It was adequate to 
implement a workable controller but still placed severe restrictions on the controller 
performance. 

The limitations mentioned above would have severe implications on the 
implementation of command and control for a cluster of UAVs. In order to support that 
objective, these constraints need to be overcome first. The proposed solution was to 
install a miniature computer in the UAV in order to minimize flight data and control 
commands transfer as well as to give the aircraft onboard computational capability to 
perform more sophisticated flight maneuver planning autonomously. This new hardware 
and control architecture implementation forms a signihcant part of this thesis work and is 
discussed in Chapter II. 
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E. 


PRIMARY INSTRUMENTS 


The FROG UAV can be configured with a variety of instruments such as air-data 
sensors, an inertial measurement unit (IMU), a GPS receiver, an instrumented nose boom 
and even a digital camera. In this project, only sensors necessary for basic aircraft 
control, navigation and communication are installed as a basic configuration. These 
instruments are introduced in the following sub-sections. 

1, Inertial Measurement Unit 

A new altitude heading reference sensor was installed on the UAV for this 
project. This is the AHRS400CA-100 manufactured by Crossbow Technology, Inc shown 
in Figure 1.6. The AHRS combines linear accelerometers, rotational rate sensors, and 
magnetometers to measure linear acceleration, angular velocity, and magnetic flux for all 
three orthogonal axes. It then utilizes a sophisticated Kalman filter algorithm to allow the 
unit to track orientation accurately through dynamic maneuvers in order to compute 
stabilized values of pitch, roll and true-magnetic heading. The Kalman filter will 
automatically adjust for changing dynamic conditions without any external user input. 
Hence, it can effectively function as the inertial measurement unit (IMU) for the aircraft. 



Figure 1.6 Crossbow Technology’s AHRS400CA-100 

The AHRS400CA-100 can be operated in 3 sensor modes and the data available 

from each mode is shown in Table I.l. In all three sensor modes, the AHRS can measure 

linear acceleration up to ±2g and angular velocity up to ±100°/sec. For this project, the 

AHRS is operated in the Angle Mode in order to utilize its Kalman filtering capabilities. 

7 



In addition, data collection can be done in either eontinuous update or polled 
mode. A detailed eomparison of data output rate in both modes was made in [4], It was 
eoneluded that in eontinuous/angle mode, the AHRS outputs data at an alternating 
frequeney of 69.7 Hz and 61.3 Hz, while in the polled mode the AHRS eould not respond 
fast enough when polled at a fixed rate of 30 Hz. As sueh, the eontinuous mode was used 
in this projeet to take advantage of the higher data rate subject to an implementation that 
would aoeommodate a varying data rate. 

The output data from the AHRS400CA-100 is provided in both digital and analog 
formats via a standard female DB-15 conneetor. The digital data is serially output via RS- 
232 interfaee at 38,400 bps and is the method used for reading data in this projeet. The 
data paeket format and output data interpretation will be deseribed in Seetion II when the 
data interfaee implementation between Crossbow AHRS and onboard eomputer is 
presented. 





Header (OxFF) 

Header (OxFF) 

Header (OxFF) 

Roll Angle 

Roll Angular Rate 

Roll Gyro Voltage 

Pitch Angle 

Pitch Angular Rate 

Pitch Gyro Voltage 

Heading Angle 

Yaw Angular Rate 

Yaw Gyro Voltage 

Roll Angular Rate 

X-Axis Acceleration 

X-Axis Acceleration Voltage 

Pitch Angular Rate 

Y-Axis Acceleration 

Y-Axis Acceleration Voltage 

Yaw Angular Rate 

Z-Axis Acceleration 

Z-Axis Acceleration Voltage 

X-Axis Acceleration 

X-Axis Magnetic Field 

X-Axis Mag Sensor Voltage 

Y-Axis Acceleration 

Y-Axis Magnetic Field 

Y-Axis Mag Sensor Voltage 

Z-Axis Acceleration 

Z-Axis Magnetic Field 

Z-Axis Mag Sensor Voltage 

X-Axis Magnetic Field 

Temp Sensor Voltage 

Temp Sensor Voltage 

Y-Axis Magnetic Field 

Time 

Time 

Z-Axis Magnetic Field 

Checksum 

Checksum 

Temp Sensor Voltage 



Time 



Checksum 




Table 1.1 Serial Outputs From AHRS400CA-100 In Various Sensor Modes 


8 




2 . 


Global Positioning System Receiver 


The GPS receiver used on the NPS Frog UAV is the Trimble Agl32 DGPS 
receiver as shown in Figure 1.7. The Agl32 DGPS is a 12 channel L-band differential 
correction receiver that provides sub-meter accuracy. It combines a GPS receiver, a 
beacon differential receiver, and a satellite differential receiver in the same housing. 
These receivers use a combined antenna with a single antenna cable. The Agl32 is 
configured with two programmable RS-232 serial ports and outputs GPS data at 1, 5 or 
10 Hz with latency of 10 msec in RS-232 serial ASCII format at baud rates up to 38,400 
bps. All outputs conform to the National Marine Electronics Association (NMEA)-0183 
data protocol. Among the various sentences in the GPS data stream shown in Table 1.2, 
only some information in the $GPGGA and the $GPRMC sentences is relevant to our 
application and is extracted for use by the flight controller and guidance algorithm. 



Eigure 1.7 Trimble Agl32 GPS Antenna and Receiver Mounted on PROG 


Message 

Contents 

GGA 

Time, position, and fix related data 

GLL 

Position fix, time of position fix, and status 

GRS 

GPS Range Residuals 

GSA 

GPS position fix mode, SVs used for navigation and DOP values 

GST 

GPS Pseudorange Noise Statisties 

GSV 

Number of SVs visible, PRN numbers, elevation, azimuth and SNR values 

MSS 

Signal strength, signal-to-noise ratio, beaeon frequeney, and beaeon bit rate 

RMC 

UTC time, status, latitude, longitude, speed over ground (SOG), date, and 
magnetie variation of the position fix 
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VTG 

Aetual traek made good and speed over ground 

XTE Message 

Cross-traek error 

ZDA 

UTC time, day, month, and year, loeal zone number and loeal zone minutes. 

PTNLDG Proprietary 

Beaeon ehannel strength, ehannel SNR, ehannel frequeney, 

ehannel bit rate, ehannel number, ehannel traeking status, RTCM souree, and 

ehannel performance indicator 

PTNLEV Proprietary 

Time, event number, and event line state for time-tagging change of state on a 
event input line. 

PTNL,GGK 

Time, Position, Position Type and DOP Values 

PTNLID Proprietary 

Receiver machine ID, product ID, major and minor release numbers, and 
firmware release date. 

PTNLSM 

Reference Station Number ID and the contents of the Special Message included 
in valid RTCM Type 16 records. 


Table 1.2 Trimble Agl32 Messages 


3, Freewave® Radio Modems 

The communieation link between the UAV and the Host Computer on the ground 
is implemented with the DGR-115 RS-232 wireless modem shown in Figure 1.8 from 
FreeWave® Teehnologies, Inc. The FreeWave modem uses frequency hopping spread 
spectrum technology and has a power output of 1/3 Watt. It is capable of communicating 
over a line of sight range of up to 20 miles, and supports data transmission at baud rates 
from 1200 bps to 115.2 Kbps. In addition, the FreeWave transceiver can operate in either 
point to point or point to multipoint modes, opening up the possibility to control of more 
than one aircraft in future. Its main specifications are shown in Table 1.3 



Figure 1.8 DGR-115 Wireless Serial Modem (left) Mounted on UAV (right) 
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ITEM 

SPECIFICATION I 

Range 

20 Miles 

RS232 Data Throughput 

1200 Baud to 115.2 Kbaud 

RS232 Interface 

Asynchronous, Full duplex 

System Gain 

135 dB 

Minimum Receiver Decode Level 

-110 dBm @ 10-4 raw BER 
-108 dBm @ 10-6 raw BER 

Operating Frequency 

902 - 928 MHz 

Modulation Type 

Spread Spectrum, GFSK 

Spreading Code 

Frequency Hopping 

Hop Patterns 

15 (User Selectable) 

Output Power 

1/3 Watt (-K25 dBm) 

Error Detection 

32 Bit CRC With Packet Retransmit 

Antenna 

3 Inch Whip Provided 

Non-standard SMA Connector Allows Use Of 
External Directional or Omni- Directional 
Antennas. 

Power Requirements 

10.5 - 18.0 VDC (Centre Positive) 

Power Consumption 

180 mA Transmit 

100 mA Receive 

120 mA Average 

Connector 

RS232 

9 Pin Female, 9 Pin Male to 9 Pin Female 
Straight Through Cable Provided 

Unit Address 

Unique, Factory Preset 

Operating Modes 

Point to Point, Point to Multipoint 

Store and Forward Repeater 

Operating Environment 

-10°Cto +50° C 


Table 1.3 Freewave® Radio Modem Specifications 


4, Differential Pressure Sensor 

A differential pressure sensor shown in Figure 1.9 (Model: 144LU04DPC) from 
Sensortechnics was also installed in the FROG UAV to measure airspeed data. It is 
capable of measuring differential pressures up to 5 inches of water and outputs 0-5V 
depending on the pressure difference between the internal static pressure and that on the 
pitot probe mounted on the nose-fairing of the UAV shown in Figure 1.10. 
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Figure 1.9 Differential Pressure Transducer 



Figure 1.10 Pitot Probe on UAV 
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II. RAPID PROTOTYPING OF NEW CONTROL SETUP 


A, PROPOSED CONTROL SETUP 


To overcome the constraints in the original setup for the conduct of the 
autonomous UAV cluster C research, a new control setup shown in Figure II. 1 was 
conceived. In this new setup, the onboard PC-104 computer would receive data from all 
airborne sensors and execute the flight control algorithm to stabilize and steer the aircraft. 
This reduces the latency time between data measurement to its use in computation, and to 
control commands output significantly. It also eliminates the need for flight data to be 
sampled at a high rate of lOOHz or more and transmitted to the computer on the ground 
for flight control computation, thereby reducing the bandwidth requirements on the serial 
modem tremendously. Instead, the onboard computer now only needs to download flight 
data to the host computer on the ground at a much lower 30 Hz to 40 Hz purely for data 
recording purposes, to still get fairly representative flight information for flight re¬ 
construction or parameter identification. As and when required, guidance commands for 
the autopilot can be sent to the flight control computer through the wireless serial modem 
link. This occurs at a much lower data rate. 


Control Surface Position Feedback 



Figure II. 1 New Control Setup 
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In an emergency, the onboard computer must quickly hand over the control 
authority of the aircraft back to the Futaba® remote control so that the pilot can regain 
control of the aircraft and pilot it from the ground. This is implemented with a PWM 
Switch that by default takes command from the Futaba® remote control unless the pilot 
grants control of the aircraft to the onboard computer. When the Futaba remote control 
commands are modified to be sent through the serial modem, its range would increase 
correspondingly with the computer control to 20 miles, and this would allow future flight 
control research work at NFS to be conducted beyond the current 1.5 mile operating 
range. 

The main constraint in the new setup is that the onboard computer must be 
powerful enough, and the data integration processes optimized sufficiently, to implement 
control at an acceptable rate. This consideration is constantly being monitored throughout 
this project and incorporated into the software drivers that are written to integrate sensor 
data. 

B, THE RAPID PROTOTYPING SYSTEM 

Implementation of the control setup was expedited using rapid prototyping 
techniques. A rapid prototyping system can be viewed as the complete set of hardware 
and software tools to implement and test the UAV’s flight controller within a reasonably 
short period of development time. In this project, a deliberate decision was taken to shift 
from the previously used RealSim® Rapid Prototyping System by WindRiver, Inc, to the 
newly acquired xPC Rapid Prototyping System by The Mathworks, Inc, as the product 
and technical support for the former is gradually being phased out. To elaborate on the 
various components of the rapid prototyping system, it would be sub-divided into several 
sections- namely, the xPC Target rapid prototyping environment, the airborne computer, 
the data I/O hardware architecture. 
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1. 


xPC Target® Rapid Prototyping Package 


xPC Target is a PC solution for prototyping, testing, and deploying real-time 
systems marketed by The Mathworks, Inc. The rapid prototyping process makes use of 
many toolboxes that are available with the popular Matlab simulation software to support 
implementation of control system simulation, data acquisition and real-time control. 
Computation and signal processing algorithms are first designed and tested as Simulink 
simulation models. xPC Target then makes use of the Real-Time Workshop® toolbox to 
convert the Simulink models into C-code, build real-time applications that can executed 
on any standard PC hardware and download into an assigned ‘Target’ PC. To interface 
the real-time application with hardware, users can make use of some of the I/O device 
drivers that comes with xPC Target to support commonly available I/O boards or develop 
their own device driver blocks by writing C-Mex S-functions in MATLAB. 



xPC Tvgct Hardware 



Figure II.2 xPC Target Setup 


In a typical setup shown in Figure II.2, a Host computer makes use of Real-Time 

Workshop to build a real-time application based on Simulink models, I/O driver blocks 

and C-code S-functions created in MATLAB and downloads it to a Target computer via 

RS232 or TCP/IP using the xPC Target environment. The application in the target 

computer can be started, stopped, or its model parameters can be changed (called ‘tuned’ 

in xPC), and run-time data can be observed or recorded on the Host or Target PC. The 
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Target PC can be a common desktop PC, a PC/104, CompactPCI, industrial PC or 
Pentium Single Board Computer regardless of operating system because xPC uses its 
own real-time operating kernel. The Target PC is diskette bootable but can be made to 
boot-up internally and initiate the ‘burnt in’ application whenever it is reset if the optional 
xPC Target Embedded Option is installed. 

The advantage of the xPC rapid prototyping system in this project is that it 
contains all the tools required to provide an integrated environment for control system 
design, software engineering, data acquisition and testing before it is finally implemented 
on the intended airborne computer. The software suite consists of the MATLAB package. 
Control System Toolbox, Simulink, Dials and Gauges Blockset, Real-Time Workshop 
and the xPC Target operating system. MATLAB and Control System Toolbox provide 
the capability to design, analysis and implement the flight controller. Simulink provides a 
graphical user interface (GUI) for construction of the controller, simulation and 
visualization. The Dials and Gauges Blockset contains a library of pre-designed blocks to 
facilitate design of a GUI for system parameters to be changed or values of variables to 
be displayed as the application is running. The Real-Time Workshop is an automatic code 
generator for the Simulink models. It converts the Simulink model and the S-Functions 
within it into C code which can then be compiled to create a stand-alone real-time 
executable program. Real-Time Workshop also schedules all the tasks to be carried out 
when the application is activated for the xPC operating system. Once compiled, the 
standalone executable code is suitable for the test-bed environment or for use in the 
embedded real-time system. The xPC real-time kernel sets up the environment in the 
Target PC to execute the application and maintain communications with the Host to 
allow parameter tuning as the application execution is in progress. 

2, Host Computer 

The Host Computer used in this project is a Pentium 111 notebook PC running 
Matlab and all the relevant rapid prototyping software toolboxes mentioned in the 
previous section and is situated on the ground. It has a standard serial COMl port for the 
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Host-Target link implementation and a Ethernet network eard for TCP/IP link-up with the 
Target PC whieh serves as a useful alternate link during development. 

3, Target Airborne Computer 

The Target PC installed on the NPS Frog UAV was re-eonfigured from the 
AC-104 computer assembled by WindRiver Systems Inc. The AC-104 computer, shown 
previously in Figure 1.3 was originally developed for WindRiver System’s own RealSim® 
rapid prototyping system. The AC-104 uses a mixture of a PC/104 compact computer I/O 
card, Industrial Pack (IP) I/O modules and a small computer motherboard. It consists of 
an Intel Pentium MMX 233 MHz processor on an Advantech PCM-5862 motherboard 
configured with 16 MB of EDO RAM and a 4 MB flash disk for non-volatile storage. 
Basic I/O is provided by a PCI-SVGA display controller, two RS-232/422/485 serial 
ports, an enhanced parallel port, keyboard controller and a PCI based lOBase-T Ethernet 
connection. 



Figure II.3 New Miniature Airborne Computer 


In the new miniature airborne computer shown in Figure II.3, the Diamond 

Systems Ruby-MM 12 bit D/A converter, the SBS GreenSprings Modular I/O Industry 

Pack IP-68322 data acquisition hardware control module and the IP-Serial board in the 

original setup were all removed. Only the Analogic AIM 16 16-bit A/D converter. Flash 

Disk RAM card and motherboard were retained. A 3.5-inch floppy drive was added to 
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boot-up the xPC Target kernel and set of Quartz-MM-5 and Quartz-MM-10 eounter/timer 
I/O boards by Diamond Systems was ineorporated to get PWM signal generation and 
eapture capability. The originally disabled COM3 and COM4 were also recovered with 
appropriate jumper settings on the motherboard and IRQ re-assignment. In addition, a set 
of switching power converters was built into the casing of the new computer to convert 
DC power from the aircraft’s batteries to various voltages for the computer, cooling fan 
and onboard instruments. The resulting miniature computer fits snugly in the front 
compartment of the FROG UAV to give it additional forward center of gravity. 



Figure II.4 Location of Miniature Computer on UAV 


Signal I/O interface between the airborne target computer, the servos that drive 
the aircraft control surfaces and control surface position sensing potentiometers connect 
to the top of the computer as shown in Figure II.5. Within the target computer, the signal 
interface specifications for the I/O boards largely fall into three categories listed below 
and are tabulated for easy reference in the corresponding Appendices. 

a. I/O Address - Appendix B.I 

b. I/O Board Pin Out Connection and Usage - Appendix B.2 

c. Interrupt Routine (IRQ) Assignment - Appendix B.3 
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Figure II. 5 Top Panel Layout of New Computer 


C. SOFTWARE INTERFACE DRIVERS 

In order for the airborne computer to compile data from onboard instruments, 
software interface drivers had to be written. A significant amount of effort was expended 
in this project to write software interface drivers that will read in and decode data output 
by the IMU and GPS receiver. This was because existing xPC serial data input block can 
only read RS-232 messages with fixed sentence structure (i.e. in 32-bit float or 16-bit 
integer format and each message must end with null character) or binary data stream with 
fixed number of characters without the capability to identify a header byte or handle 
varying number of bytes in messages. On the other hand, to work with the IMU and GPS, 
the interface drivers must be capable of searching for appropriate header byte or header 
string in a continuous stream of RS-232 data while performing checksum computation in 
real-time to validate the data and applying appropriate formula to decode the incoming 
data. Therefore, instrument specific software interface drivers had to be written. Details 
of the driver implementation for the case of the CrossBow IMU and Agl32 GPS is 
discussed next. 
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1 . 


IMU Data Interface 


In the Crossbow AHRS400CA-100, the digital data representing each 
measurement is sent as a 16-bit number (two bytes). The data is sent in ‘Big Endien’ 
format, i.e. MSB first then LSB. In Angle Mode, the data generally represents a quantity 
that can be positive or negative and is sent as a 16-bit signed integer in 2's complement 
format. Only the timer information and temperature sensor voltage are sent as unsigned 
integers. Each data packet begins with a header byte (255) and ends with a checksum. 
Hence, for the Angle Mode used in this project, each message consists of 30 bytes 
inclusive of the header byte, 14 data values and the checksum as shown in Table II. 1. 

a. IMU Data Receive Implementation 

Several characteristics peculiar to the Crossbow AHRS message data 
structure shown in Table II. 1 dictated the way the software driver receiving serial data 
from Crossbow had to be written. First, the message rate (i.e. number of message 
Crossbow outputs) in Continuous Data Collection Mode is not constant. It was measured 
to be fluctuating between 69.7 Hz and 61.3 Hz in [4]. This precludes the use of a standard 
message retrieval rate (e.g. fixed at 65 read per second) by the airborne computer if we 
intend to read and utilize every piece of data output from the instrument. Second, the 
header byte FF will not be the only FF byte in the each data packet because FF bytes can 
occur in the message body. Therefore, the receiving software driver must count the bytes 
received from the serial port and use the checksum to determine if a particular FF byte is 
a header or just another byte in the body of the message. The implemented Crossbow data 
receive and decode Simulink block diagram is shown in Figure II.6. 



RS232 1 


Figure II.6 Crossbow AHRS Data Receive and Decoding Block Diagram 
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The ‘Xbow Receive' block calls the data receive driver xbowrcvlpS.c in 
Appendix C. xbowrcvlpS.c uses a soft-buffer to collect every byte received by the COM 
port serial buffer and progressively check every byte in the soft-buffer for a FF byte. 
Upon locating each FF byte (possible message header), the routine computes the 
checksum of all data bytes (Byte 2 to Byte 29 in Angle Mode) to assess if the computed 
checksum tallies with the transmitted checksum byte (Byte 30 in Angle Mode case). If 
the checksum does not tally, that FF byte is not a header and the routines goes in search 
of the next FF byte. If the checksum tallies, the routine outputs the full message (Header 
byte + 28 data bytes + checksum byte in Angle Mode case) and shift unused bytes to the 
front of the soft-buffer for processing at the next time-step. If there is inadequate data to 
form a message (this occurs if the sampling interval is much smaller than data rate), the 
previous message is returned during each code execution time-step. If on the other hand, 
the sampling interval is longer than data rate, and the data in soft-buffer exceeds 100 
bytes (i.e. about 3 messages), the soft-buffer is flushed to allow new incoming bytes to 
reach the front of soft-buffer for processing. This will assure the ‘freshness’ of data. As 
such, the implemented driver is able to collect every useful byte of data and operates 
independently of the message rate from the IMU. 


Byte VG Mode Scaled Sensor Mode Voltage Mode 


0 

Header (255) 

Header (255) 

Header (255) 

1 

Roll Angle (MSB) 

Roll Angular Rate (MSB) 

Roll Gyro Voltage (MSB) 

2 

Roll Angle (LSB) 

Roll Angular Rate (LSB) 

Roll Gyro Voltage (LSB) 

3 

Piteh Angle (MSB) 

Piteh Angular Rate (MSB) 

Piteh Gyro Voltage (MSB) 

4 

Piteh Angle (LSB) 

Piteh Angular Rate (LSB) 

Piteh Gyro Voltage (LSB) 

5 

Heading Angle (MSB) 

Yaw Angular Rate (MSB) 

Yaw Gyro Voltage (MSB) 

6 

Heading Angle (LSB) 

Yaw Angular Rate (LSB) 

Yaw Gyro Voltage (LSB) 

7 

Roll Angular Rate (MSB) 

X-Axis Aeeeleration (MSB) 

X-Axis Aeeel Voltage (MSB) 

8 

Roll Angular Rate (LSB) 

X-Axis Aeeeleration (LSB) 

X-Axis Aeeel Voltage (LSB) 

9 

Piteh Angular Rate (MSB) 

Y-Axis Aeeeleration (MSB) 

Y-Axis Aeeel Voltage (MSB) 

10 

Piteh Angular Rate (LSB) 

Y-Axis Aeeeleration (LSB) 

Y-Axis Aeeel Voltage (LSB) 

11 

Yaw Angular Rate (MSB) 

Z-Axis Aeeeleration (MSB) 

Z-Axis Aeeel Voltage (MSB) 

12 

Yaw Angular Rate (LSB) 

Z-Axis Aeeeleration (LSB) 

Z-Axis Aeeel Voltage (LSB) 

13 

X-Ax is Aeeeleration (MSB) 

X-Axis Magnetie Field (MSB) 

X-Axis Mag Voltage (MSB) 
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14 

X-Axis Acceleration (LSB) 

X-Axis Magnetic Field (LSB) 

X-Axis Mag Voltage (LSB) 

15 

Y-Axis Acceleration (MSB) 

Y-Axis Magnetic Field (MSB) 

Y-Axis Mag Voltage (MSB) 

16 

Y-Axis Acceleration (LSB) 

Y-Axis Magnetic Field (LSB) 

Y-Axis Mag Voltage (LSB) 

17 

Z-Axis Acceleration (MSB) 

Z-Axis Magnetic Field (MSB) 

Z-Axis Mag Voltage (MSB) 

18 

Z-Axis Acceleration (LSB) 

Z-Axis Magnetic Field (LSB) 

Z-Axis Mag Voltage (LSB) 

19 

X-Axis Magnetic Field (MSB) 

Temp Sensor Voltage (MSB) 

Temp Sensor Voltage (MSB) 

20 

X-Axis Magnetic Field (LSB) 

Temp Sensor Voltage (LSB) 

Temp Sensor Voltage (LSB) 

21 

Y-Axis Magnetic Field (MSB) 

Time (MSB) 

Time (MSB) 

22 

Y-Axis Magnetic Field (LSB) 

Time (LSB) 

Time (LSB) 

23 

Z-Axis Magnetic Field (MSB) 

Checksum 

Checksum 

24 

Z-Axis Magnetic Field (LSB) 



25 

Temp Sensor Voltage (MSB) 



26 

Temp Sensor Voltage (LSB) 



27 

Time (MSB) 



28 

Time (LSB) 



29 

Checksum 




Table II.1 Serial Data Structure From Crossbow AHRS400CA-100 


b. Decoding Crossbow Data 

The decoding routine simply takes in the float value of consecutive two 
bytes after the header byte, multiplies the first byte of each pair by 256, adds it to the 
second byte to obtain the numerical value for data items transmitted in 2’s complements. 
Each set of data (e.g. angles, rates, acceleration and magnetic field) is then scaled 
according to Simulink implementation structure shown in Figure II.7. The factors AR, 
GR and MR are specific to each Crossbow AHRS and are given in the factory calibration 
data of the instrument. An example of the decoded data for Euler angles and rates is 
presented in Figure II.8 when the Crossbow IMU was rotated in the roll, pitch and yaw 
axis sequentially by approximately 80° in each direction to check the validity of the 
decoded data. 
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Figure II.7 Simulink Block to Decode Crossbow Data 




Figure II.8 Decoded Euler Angle and Rates from Crossbow 
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2 . 


GPS Data Interface 


The Agl32 receives GPS messages in NMEA-0183 format. The messages are 
essentially strings of comma-delimited text shown in Figure II.9 below. Each NMEA 
message includes a message ID to distinguish the message from other NMEA messages 
in the data stream. The actual data included in NMEA-0183 messages is placed in fields. 
Each NMEA message contains different number of fields, and each field is preceded by a 
comma character. The messages include a checksum value which is useful for checking 
the integrity of the data included in the message. The checksum is the 8-bit exclusive OR 
of all the characters in the message, between the and delimiters. 


Aslaisk Daimla - 
rComma Dellnila 

$GPZDa!220320.0.2G.06.1997 .00.00-52 



Figure II.9 NMEA-0183 Message Structure 

a. Receiving GPS Message Strings 

The manner in which GPS messages are received the airborne computer is 
shown in Figure II. 10 . The GPS Receive block executes the gpsrcv.c software driver in 
Appendix C to read every byte of the GPS data into a storage and output buffer system 
(similar to that describe for the Crossbow IMU so that no data byte is lost when sampling 
is done at data rates higher than the incoming GPS data), evaluates the 5-byte header 
following each character, identifies if a GPGGA or GPRMC message has been 
received and returns a Header Index to identify the type of message. It also searches 
through the message to look for the end of message delimiter bytes (OxOD, OxOA) which 
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mark the end of eaeh message and outputs all the bytes between the header string and 
delimiter bytes. 



Figure II. 10 GPS Message Reeeive Bloek Diagram 


b. Decoding the GPS Messages 

Message deeoding is done by two bloeks following the GPS receive 
model as shown in Figure II.ll. The GCA and RMC bloeks are enabled to exeeute the 
gpgca.c and gprmc.c routines in Appendix C when the Header Index from the 
GPS receive model matehes the message type eaeh bloek is assigned to deeode. Sinee 
each data field is separated by the comma character, the routine extracts each field of 
useful data from the received binary data sent by the GPS receive model based on the 
expected format and contents of fields for the GPGGA and GPRMC messages as shown 
in Table II.2 and Table II.3 respectively. 


CT>- 

HeaderType 



CI> 

GPS data 


—► 


dational I 

npratnM_▼_ 


n 


In1 

RMC 


►dD 

GPRMC 


Figure II. 11 GPS Message Decoding Block Diagram 


Field 

Description 

1 

UTC of position fix in HHMMSS.SS format 

2 

Latitude in DD MM,MMMM format (0-7 decimal places) 
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3 

Direction of latitude 

N: North 

S: South 

4 

Longitude in DDD MM,MMMM format (0-7 decimal places) 

5 

Direction of longitude: 

E: East 

W: West 

6 

GPS Quality indicator 

0: fix not valid 

1: GPS fix 

2: DGPS fix 

7 

Number of SVs in use, 00-12 

8 

HDOP 

9 

Antenna height, MSL reference 

10 

M’ indicates that the altitude is in meters. 

11 

Geoidal separation 

12 

‘M’ indicates that the geoidal separation is in meters 

13 

Age of differential GPS data record. Type 1. Null when DGPS not used 

14 

Base station ID, 0000-1023 


Table II.2 GGA Message Fields 


Field Description 

1 

Time: UTC time of the position fix in hhmmss.ss format 

2 

Status 

A: Valid 

V: Navigation Receiver Warning (V is output whenever the 

Receiver suspects something is wrong) 

3 

Latitude coordinate 

(the number of decimal places, 0-7, is programmable and determined by the numeric 
precision selected in TSIP Talker for a RMC message) 

4 

Latitude direction: N = North, S = South 

5 

Longitude coordinate (the number of decimal places, 0-7, is 

programmable and determined by the numeric precision selected in TSIP Talker for a 
RMC message) 

6 

Longitude direction 

W: West 

E: East 

7 

Speed Over Ground (SOG) in knots (0-3 decimal places) 

8 

Track Made Good, True, in degrees 

9 

Date in AAlmmlyy format 

10 

Magnetic Variation in degrees 

11 

Direction of magnetic variation 

E: Easterly variation from True course (subtracts from True 
course) 

W: Westerly variation from True course (adds to True course) 

12 

Mode Indication 

A: Autonomous 

D: Differential 

N: Data not valid 


Table II.3 RMC Message Fields 
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D. 


COMBINED I/O TEST 


The Simulink block diagram for the combined I/O test involving GPS 
data, IMU data, PWM generation, PWM capture and host-target communication 
via serial modem is shown in Figure 11.12. It is driven by user inputs from the 
display screen in Figure 11.13 created using Dials & Gauges. The PWM signal 
generated by the computer (shown in Figure 11.14) was designed to emulate that 
generated by the Futaba® remote control system which uses a pulse period of 
approximately 14.25 milliseconds and a pulse width varying from around 0.9 to 
2.2 milliseconds depending on the command input. 



Figure II. 12 Block Diagram For Combined Test 


The computer takes only an average of 350 microsecond to compile the GPS, 
IMU, A/D sensor data, output PWM signals, measure PWM signals and display results to 

the VGA monitor at each time step. This execution time can further be reduced if display 
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of data on the xPC scope is removed. Hence, the data collection and processing time on 
the airborne computer has improved significantly over the original setup where data was 


updated only at 25 Hz. 



Figure 11.13 Combined Test User Interface Screen 
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Output High Voltage = 4.72V (ok) 



Figure 11.14 PWM Signal Generated by Computer Matches Command 
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III. FLIGHT CONTROLLER DESIGN 


For the controller design effort, a six degrees-of-freedom (6-DOF) model of the 
NFS FROG UAV was first developed in Simulink based on available flight 
characteristics data collected in [6]. The flight controller was then designed using 
classical control inner-outer-loop approach using the dynamic model of the FROG UAV 
around the trimmed flight condition and adjusted for non-linearity with gain-scheduling 
using dynamic pressure as the scheduling parameter. The inner-loop-outer-loop structure 
was chosen as the first controller to be implemented because its structure made it easier 
to conduct checks on the aircraft. Another controller was designed using the Integral 
Linear Quadratic Regulator (Linear LQR) Synthesis method described in [8] to serve as 
performance comparison. 


A. 6DOF AIRCRAFT MODEL DEVELOPMENT 

The derivation of equations of motion for a 6-DOF aircraft model can be divided 
into 2 main parts. The first part is the formulation of equations of motion for the aircraft, 
treated as a rigid body, in space. These dynamics are typically common for any rigid 
body. The second part is the computation of aerodynamic, gravitational and thrust forces 
on the aircraft. These forces, except for gravitational, are specific to each aircraft and 
depend directly on its stability and control derivatives as well as on engine performance. 

1. Equations of Motions 

The equations of motion for the linear dynamics and angular dynamics of the 
aircraft is developed as follows: 

a. Linear Dynamics Equations 

The equations for linear motion are governed by Newton’s Second Law, 
F = ma , expressed in the inertial frame {U} . However, aircraft velocities and attitude 
angles are usually measured with respect to the aircraft’s body axis coordinate system 
{B} . Linear forces and accelerations are also typically expressed in {B} . The origin of 
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the body axis {BO) is taken to eoineide with the aireraft eenter of gravity (e.g.), henee the 
position of aireraft e.g. with respeet to inertial axis is denoted ^Pgo and its veloeity is 
given by 


% = 

'^BO BO 


(III.l) 


Pre-multiplying by the rotation matrix from {U} to {B} gives 


^ D A ^ p ^ p 

^BO ~ ^BO 


(111.2) 


where the rotation matrix is given by 


B 

U 


R = 


cos T cos© 

cos^ sin©sinO - sinTcosO 
cosTsin©cos<I> + sinTsin<I) 


sin ^ cos© -sin© 

sin T sin © sin O + COST cos O cos© sin O 
sin T sin © cos O-COST sin O cos©cos<J> 


(III.3) 


or equivalently as 


B Bn 

^BO ~ BgQ 


(111.4) 


The total derivative of a veetor ^4 in a rotating eoordinate system with angular veloeity 
CO, is given by Coriolis’ theorem 


A = —A + cox A 
dt 


(III.5) 


Henee, the rigid body’s linear aeeeleration is given by 


+ ^cOu X 


dt 


''BO ^ '^B '' ''BO 


(III.6) 


Newton’s law applied to the aireraft e.g. ean then written as (111.7) resolved in inertial 
frame or as (III.8) resolved in body axes. 


r = a 


BO 


u 

= m V 


BO 


(111.7) 


9 7 —' B j~} u • 

F = m^R Vgo 


B 

= m V 


BO 


(III.8) 
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Finally, the equation to be modeled is given by (111.9) after ineorporating (III.6) into 
(III.8). 

V = + (III.9) 

\dt J 

b. Angular Dynamics Equations 

The angular equations of motion are derived using Euler’s law for 
eonservation of angular momentum in the inertial frame where denotes moment 

imparted to the rigid body and ^Lg^ denotes the rate of ehange of angular momentum. 

^NgO= "LgO (in.lO) 

Again using the rotation matrix to body axes and applying Coriolis’ Theorem 

^Lg, = iR^Nga (III. 11) 

•i„ = (III. 12) 

at 

where the angular momentum term ^Lg^ is defined as the produet of the inertia 

tensor and the body’s angular veloeity plus the inertia tensor of the spinning rotor on the 
aireraft and the rotor’s angular veloeity all with respeet to body axes. 

^ (III.13) 

Substituting (III. 13) into (III. 12) gives 

= ^(//®, + 4H) + Xx(4X + 4X) (III.14) 

In this projeet, Ig^cOg is also negleeted. If we further assume that Ig,Ig and 
^cOg are eonstant, (III. 15) results. 

XO = l/(bg + %gX(lg^O}g+Ig^COg) (III.15) 
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c. State Equations 

In summary, the kinematics equations to be implemented in the rigid body 
6DOF model is governed by (III. 16) and is re-written in matrix form as (III. 17). 


B 

B 


F 

N 


IB (i>g + (Og X g COg + Ig COg ^ 


(III. 16) 


which can be re-arranged and implemented in linear state space form as 


\go 

dt ’^(Og 


^ ^BO 


Ig COg X ^Ig COg + Ig COg ^ 


+ 

+ 


m 

r^N 


(III. 17) 


2, Forces and Moments on Aircraft 

In order to compute the ^F and as required in (III. 17), the total contribution 
of forces and moments exerted on the aircraft due to aerodynamic, propulsion and 
gravitational effects is computed using the expression (III. 18). 

"V 


p +^p +^p 

Aero Prop Prop 

^^Aero~ ^^?xop 


(III. 18) 


a. Aerodynamic Forces and Moments 

The aerodynamics force and moment terms are determined using a first- 
order Taylor series expansion around the aircraft trimmed operating point. Each term in 
the series is a partial derivative of ^F and with respect to the aerodynamic variables 

— ,a,p,p,q,r i.e. 

Vf 

FA.ro = dF^x^5F.x^5F,S^F, (III.19) 

FA.ro = 5F^x^5FJ^5F,S^F, 


where 
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X 


u 


(III.20) 


2V^ 2V^ 


rb 

2V^ 


X 



(III.21) 


A = 


(III.22) 


The forces and moments generated by aerodynamic forces with respect to the 
wind-axis as denoted in (III.23) and the rotation matrix defined in (III.24) can be 

applied to obtained and . 





qS\C^ 


DC . dC .. 5C , 1 

+-rX +-rX +-A 1 

dx dx dA J 


(III.23) 


B 

w 


R 


COSO'cos/? COSO'sin/? 

- sin P cos P 

sin a cos P sin a sin P 


-sinci' 

0 

COSfl' 


(III.24) 


The forces resulting from drag and lift are taken negative to obtain positive signed 
forces in the x^ (i.e. forward) and (i.e. down) directions. 


wp _ 

Aero 

1 

1____ 

and = 

m 


l-L\ 


n 


(III.25) 


The rest of the quantities in (III.23) are defined as follows: 


dC^ 

dx' 


c c c c c c 

'-'Du '-'Df) '-'D, '-'D^ '-'E 

C, C, C, c, C, Cv 


c, c, c, c, 


c, 

rriu 

c„ 


c, c, 


c„ 


c„ 


Q 

p 

c„ 


c, c, 


c„ 


^4 

c„ 


(III.26) 
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" 0 

Co. 

Co. 

0 

0 

0 


0 

Q 

Cy. 

‘-a 

0 

0 

0 

dc _ 

0 

Q 

A 

Q 

0 

0 

0 

dx' ~ 

0 

Q 

7^ 

Q 

‘a 
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0 

0 


0 

C,.„ 
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0 

0 


0 



0 

0 

0 


(III.27) 


dA 




(III.28) 




-5 0 0 0 0 0 

0 5 0 0 0 0 

0 0 -5 0 0 0 

0 0 0 5 /) 0 0 

0 0 0 0 5c 0 

0 0 0 0 0 5/) 


(III.30) 


(III.31) 
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The aerodynamic variables x are typically not used as states in the 
computation of and . Instead, the set of orthogonal linear velocities and angular 
velocities in (III.32) which are easily measured on the aircraft is commonly used. 

X = \u V w p q (III.32) 


Transformation matrices M and M relating the chosen state vector x to the 
aerodynamic variables x and x are defined as follows: 

M : X ^ X 
M :x ^ X 

where 



0 0 0 
0 0 0 


0 0 
0 0 
0 0 



0 0 0 0 0 


0 

0 

0 

0 

0 



(III.33) 


0 


M' = 


0 




0 0 0 
0 0 


(III.34) 


The complete expression for and can then be written as 




0 


0 


'.R 


dC 


dc 


dc 


^ S s H pM X H —X H-A 


dx 


dx 


0A 


(III.35) 
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b. Gravitational and Propulsive Forces and Moments 
Gravity contributes forces on the rigid body but no moments since the 
forees are assumed to aet at the eenter of gravity. The gravitational forees aeting on the 
aireraft are obtained simply by pre-multiplying by the appropriate rotation matrix 

as follows: 


F = 

Grav 


0 

0 

mg 


(III.36) 


then 


- B 

^ Grav U 


R 


0 

0 


mg 


(III.37) 


Propulsive forees and moments are exerted in {B} axis and is simply eomputed 
direetly based on the expressions 


F = 

^ Prop 


N 

^ ’ Prop 


T 

m 


(III.38) 


(III.39) 


where Tfs represents forees or moments due to propulsive thrust. Obviously, the 
propulsive forees and moments depend on the engine installation and must be determined 
for eaeh aircraft individually. For the purpose of this project, the thrust forees in ^y, ^z 
and moments are considered negligible and are ignored. 


The eomplete forees and moments exerted on the aireraft are thus given by 

(III.40). It is implemented in Simulink using bloek diagrams shown in Figure III.l and 
forms the input to the Equations of Motions portion of the 6DOF model. 


B 

B 


F 

N 


:r 

0 


:R 


qS 



1 A-' 

+ —vM X + 

dx 


—vM x + 
dx 



1 

'Bp 


'Bp 

1 

+ 

^ Grav 

+ 

^ Prop 


J 

0 


0 

J 


(III.40) 
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3 


6DOF Model of FROG UAV in Simulink 


The Simulink models implementing Equations (III. 17) and (III.40) are shown in 
the following figures. This non-linear 6DOF model of the FROG FlAV was then used to 
design eontrollers for the aireraft. 



6 Degrees of Freedom Equations of Motion for a Rigid Body 
(Euier angies) 



[Angular Velocity Equations' 


_ Ru2b 

^ule^ngle^^quation^j EuierAngies 
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Computation of Forces and Moments Experienced by Aircraft 



Figure III.l Simulink Blocks Implementing 6DOF Model of FROG UAV 


B, CLASSICAL CONTROLLER DESIGN 

The first autopilot was designed based on classical inner/outer loop methodology 
using the linearized model of the 6DOF FROG UAV developed in the previous section. 
Actuator models representative of those driving surface actuators and throttle on the 
aircraft with saturation limits imposed were included before the non-linear FROG model 
to better emulate the actual hardware response. The procedure consists of evaluating the 
stability of the feedback loop using root locus techniques, adding poles or zeros to shape 
the system behavior in the compensator where needed, adjusting loop gains to achieve 
desired gain and phase margins and verifying the response in each loop with step 
commands of reasonable magnitude. 

General requirements were to attain more than 6dB gain margin, at least 45° 

phase margin and at least one decade bandwidth separation between inner and outer 
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loops. The control channels were designed in the order: yaw damper, speed control, 
altitude control followed by heading control 

1, Yaw Damper Design 

The yaw damper was designed first in order to prevent the aircraft from yawing 
due to its lightly damped dutch-roll mode when it executes a longitudinal maneuver. This 
loop was comparatively more difficult than the rest due to the presence of low damping 
zeros near the origin and a unstable pole from the spiral mode. As shown in Figure III.2, 
the root locus for the yaw damper near the origin would result in instability or very poor 
damping due to the zeros at -0.0834±0.8298i with a simple gain feedback. Poles had to be 
added at -0.5 and -0.3, with zeros at -0.5000±1.0000i to bring the locus quickly into the 
stable region and give good damping as shown in the second root locus in Figure III.2. 

For turn coordination, a computed rate bias of gtan^zi^/Fj, was imposed as a 
command to the PI controller in the yaw damper as described in [7] so that yaw damper 
would not attempt to counter a commanded turn. The final structure of the yaw damper is 
shown in Figure III.3. The airplane response responses to yaw rate disturbance inject of 
0.2 rad/s and commanded yaw rate of 0.2 rad/s is presented in Figure III.4. The yaw rate 
feedback method does not give extremely good responses due to the influence of the 
unstable pole from the spiral mode. 


Root LockM of V«i> Dampor MNlhoul CompontMo* 



.17 .1 .04 «7 0 a? 04 

Roof Ai» 


Rom Locu* of l&m Oonpor Wkh CompooMior 



Ro«i Am 


Figure III.2 Root Locus of Yaw Damper With and Without Compensator 
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Yaw Damper 



Compute r for 
turn rate coordination 


Yaw Stabilization~|| 


Figure III.3 Yaw Damper Bloek Diagram 




Figure III.4 Yaw Damper Responses 


2, Speed Controller 

A simple PI eontroller was implemented for speed eontrol as shown in Figure 
III.5. This was enough to give a phase margin 91.8° @ 0.42 rad/s with infinite gain 
margin. Figure III.6 shows that the aircraft would be able to track a commanded 10 fps 
change in airspeed in about 10 seconds. 
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Speed Controller 



Figure III.5 Speed Control Block Diagram 




Figure III.6 Speed Controller Responses 


3, Altitude Controller 

Altitude control entails driving the elevator of the aircraft to pitch the aircraft up 
or down in a climb or dive maneuver to execute desired altitude changes. To achieve this, 
the aircraft must first be capable of pitch control. Hence, the pitch loop is often referred 
to as the inner loop and is designed first. The pitch control loop consists of a PID 
compensator on the pitch attitude error (0e) while the outer loop employed another PID 
compensator with altitude error, which generates the pitch command for the inner loop. 
Additionally, a compensator with pole at -25 and zero at -15 had to be inserted to delay 
the system poles from crossing into the right half plane. This was because the 
longitudinal modes originally had a real zero at s=19.4 and would result in an odd 
number of poles and zeros to the right of the origin and the integrator pole from the outer 
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loop PID would go right immediately otherwise. The resulting eontroller structure and its 
root locus is presented in Figure III.7 and Figure III.8 respectively. 


Altitude Controller 



theta 


Outer Loop - Altitude Control 


Inner Loop - Pitch Control 


Figure III.7 Altitude Controller Block Diagram 
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Figure III.8 Root Locus of Altitude Controller With Compensator 
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Figure III.9 shows the pitch control loop responses to commands from the outer 
loop and its disturbance rejection property. In both cases, when 0.2 rad changes were 
input, the elevator deflected a reasonable 0.1 rad or 5°. Figure III. 10 shows the aircraft 
model response to a commanded 10 ft change in altitude. The aircraft settled in its new 
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altitude within 15 seconds. Elevator responded in negative direction as expected with 
about 25% increase in throttle required to execute the climb. The bode plots for both 
inner and outer loops are shown in Figure III.l 1. 



Figure III.9 Pitch Control Foop Responses 




Figure III. 10 Altitude Control Foop Responses 


hich CoMrol Loop Sod* Ow^am Con»i*«id Loop Sod* Plot 





Figure III. 11 Altitude Control Inner and Outer Foops Bode Plots 
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4, Heading Controller 

The heading eontroller was designed in a similar fashion to the altitude eontroller. 
The outer loop took commanded heading angle error and produced an angle of bank 
command. The inner loop took the bank command to generate the required aileron 
deflection. The inner loop consisted of a PI controller on the bank angle error and a roll 
rate feedback, while the outer loop consisted of a PI controller to null the heading error. 
The controller structure is shown in Figure III.12. Figure III.13 shows that the controller 
was able to track roll commands within 5 seconds and heading commands within 20 
seconds. 


HEADING CONTROLLER 



P 


[lnnerLoop^Bank_Cont^ 


Figure III. 12 Heading Controller Block Diagram 



Figure III. 13 Roll and Heading Control Loop Responses 
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Figure III. 14 Heading Control Inner and Outer Loops Bode Plots 


5, Complete Controller 

The eomplete controller was implemented as shown in Figure III. 15. The gain and phase 
margins and closed-loop bandwidths are given in Table III.l below. 


Loop 

GM (dB) 

PM (deg) 

CLBW (rad/s) 

Yaw Damper 

14.2 

46.3 

30 

Vt 

Inf 

91.8 

0.65 

Pitch Control 

12.2 

57.0 

9.27 

Altitude Commad 

14.7 

71.6 

0.88 

Roll Control 

8.6 

35.6 

14.5 

Heading Command 

6.75 

67.7 

0.78 


Table III.l Classical Controller Bandwidth Gain and Phase Margins 


Flight Controller for Non-Linear 6DOF Model of FROG UAV 



Figure III. 15 Complete Flight Controller using Classical Control Design 
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C. LQR CONTROLLER DESIGN 


The second flight controller was designed to control 4 variables in steady-state 
flight - namely, airspeed (Vt), sideslip (v), heading (\|/) and altitude (h). The design was 
done using an Integral LQR structure with state-feedback and is based on the technique 
presented in [8]. The design requirements adopted were: 

□ Zero steady state errors to constant command in airspeed, sideslip, heading 
and altitude. 

□ Overshoot to step commands in altitude and airspeed must not exceed 10%. 

□ Rise time in response to step altitude commands and step airspeed commands 
should be around 10 sec. 

□ Gain margin in control loops should be at least 6 db and phase margins at least 
45 degrees. 

□ Aileron, elevator and rudder loop bandwidth should be around 10 rad/sec. 
Thrust loop bandwidth should not exceed 5 rad/sec. 

The asymptotic property of the LQR controller inherently ensures a stable 
feedback system. The optimal feedback gain Kopt = generated by the LQR method 

minimizes the cost function, 

J = (x^Qx + u^Ru) dt (111.41) 

and assures that the closed loop system is stable, assuming that (A,B) is 
stabilizable, and (Q,A) is detectable. The matrices Q>0, R>0 are weighting matrices, 
which determine the relative cost of error and energy in the states and control inputs. The 
P>0 matrix is the unique stabilizing solution to the Algebraic Riccatti Equation. 

A^P + PA - PBR-^B^P + g = 0 (111.42) 

To ensure zero steady state errors to constant altitude and airspeed commands, the 
integral control is used in conjunction with the LQR technique. The altitude error {he - h) 
and the airspeed error {uc - u) were fed to integrators. The integrating action will ensure 
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that the inputs to each integrator, and hence the altitude and airspeed errors, will be 
driven to zero in steady state in response to constant commands. 

1. Stabilizable and Detectable Criteria 

To design the controller using LQR (linear quadratic regulator) technique, the 
plant must be stabilizable and detectable. A check on the 12-state \uvwpqr(j)0 y/Px Py 
Pz] aircraft model for the 4 controlled variables showed that only 10 states where 
observable. As such, the 10 state equation of motion model with only {uvwpqr(/)6y/ 
Pz\ was used for controller design using the step as follows. 

x = [uvwpqr(p 6 y/ P^X 

□ Construct the synthesis model for the plant. 

□ Insert transmission zeros to the synthesis model. (This will be the ‘target’ 
poles location for the state-feedback plant.) 

□ Linearize the synthesis model. 

□ Adjust the Q and R matrices to vary the cost of states and control inputs, (start 
with identity). 

□ Compute the optimal gain K using MATLAB’s “lqr(A,B,Q,R,N)”. 

□ Insert the optimal gain K for the plant’s states and error states feedback. 

□ Repeat the last 2 steps while adjusting Q and R to achieve desired control 
bandwidths, gain and phase margins. 

2, Synthesis Model and Controller Structure 

The synthesis model and controller structure are shown in the next few figures. 
The Matlab code created to compute the feedback gains, closed-loop response, 
bandwidth, gain and phase margins can be found in Appendix C. 
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Heading Controller 


Figure III. 16 Overview of Synthesis Model for Controller 



Figure III. 17 Creating Real Synthesis Pole 



Figure III. 18 Creating Complex Synthesis Pole 
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Figure III. 19 Linear Integral LQR Controller Structure 


For the selected design, two pair of transmission zeros were added in the h- 
synthesis loop (at ^ = 0.9, wn = 0.4) and v-synthesis loop (at ^ = 0.9, wn = 1.2), and a 
real pole each for Vt and \|/ at 0.7 rad/s and 1.5 rad/s respectively. The diagonal elements 
of the Q and R matrices were set to identity and [2000,5000,1500,2] respectively. The 
3dB closed-loop bandwidths and open-loop gain and phase margins achieved are shown 
in Table IIL2. In comparison with the classical controller, the Integral LQR controller 
offers better phase margins. However, it had assumed full-state feedback and may not be 
directly implementable if any of the 10-states listed previously is not easily measurable or 
tends to fluctuate. 


Control Loop 

3dB BW (rad/s) 

Gain Margin (dB) 

Phase Margin (°) 

Aileron 

4.8 

12 

78.9 

Elevator 

8.2 

18 

52.3 

Rudder 

1.2 

15 

91.3 

Throttle 

4.6 

21 

69.7 


Table III.2 LQR Controller Bandwidth Gain and Phase Margins 


49 


























3, Complete LQR Controller 

The non-linear system simulation structure adopted in Figure III.20 was based on 
a method described in [9]. The method is based on the observation that the linear 
controller obtained is designed to operate on the perturbations of the plant’s input and 
outputs about the trimmed condition. Differentiating the measured outputs before they are 
fed to the gains extracts out the perturbations for which the gains are designed to operate 
on. To preserve the input-output behavior of the feedback system, a integrator is inserted 
after the feedback gains are computed. The performance of the LQR controller on the 
non-linear FROG UAV model thus obtained is shown in Figure III.21. 


Integral LQR Controller for FROG UAV 



Figure III.20 Non-Linear LQR Controller Implementation 
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Figure III.21 LQR Controller Performanee with Non-linear UAV Model 
a) Airspeed Change of 12ft/s b) Altitude Change of 50 ft 

e) Sideslip Veloeity of 10 ft/s d) Heading Change of 0.2 rad 

D, CONTROLLER COMPARISON 

Coupling between the longitudinal and lateral eontrol modes was observed on 
both elassieal inner-loop-outer-loop eontroller and integral LQR eontroller. The 
eontroller response and performanee ean be analyzed based on the sign eonvention used 
in [10] and shown in Figure III.22. 
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Figure III.22 ANSI/AIAA Sign Convention for Control Surfaee Defleetion [From 10] 


In the aircraft with a classical controller, the aircraft’s response to an altitude 
increase of 20 feet in Figure III.23 shows typical longitudinal mode coupling. The 
controller input a elevator up (negative 5e) command to tilt the aircraft nose up 
immediately upon receiving the altitude increase command, airspeed dropped to 85 fps 
and the throttle had to be increased by about 50% above trim level to bring the airspeed 
back to 88 fps. Figure III.24 shows the reverse coupling relation. A step command of + 12 
fps sent to the speed controller causes the throttle to open and aircraft to accelerate. As 
the aircraft speeds up, it generates more lift and as a result begins to climb by up to 6 feet. 
The altitude controller immediately commands a positive 5e to arrest the climb and 
attempt to return the aircraft to its original altitude. As it returns to the original altitude, 
the throttle input stabilizes at 20% about trim value to maintain the new airspeed. 

Lateral coupling can be observed in Figure III.25. When a step command of +0.2 
radian was input to the heading controller, the controller immediately issued a positive 
aileron control input to bank the aircraft right. The yaw controller simultaneously issued 
negative 5r input for the rudder to provide turn coordination as designed. 
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Figure III.23 Classical Controller - Response to Altitude Change of +20 feet 



Figure 111.24 Classical Controller - Response to Speed Change of+12 fps 





Figure III.25 Classical Controller - Response to Heading Change of +0.2 rad 
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Similar kind of coupling effects can be observed in the Integral LQR controller. 
However, with full-state feedbaek, the integral LQR eontroller is able to minimize the 
eoupling effeets better. For example, as shown in Figure III.26, although the negative 6e 
and positive Sthrottie were still issued by the eontroller to exeeute the altitude change, the 
throttle input was issued earlier compared to the elassieal eontroller and resulted in only 
minimal drop in the aireraft speed as the aireraft elimbed to its eommanded new altitude. 
The eoupling effeet on altitude was also mueh redueed when a speed ehange was 
eommanded. This is shown in Figure III.27. The altitude ehanged less than 1 feet in the 
integral LQR eontroller aireraft versus 6 feet in the elassieal eontroller aireraft. 



Figure 111.26 LQR Controller - Response to Altitude Change of +20 feet 




Figure 111.27 LQR Controller - Response to Speed Change of+12 fps 
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Similarly, the lateral coupling effect is much reduced in the integral LQR 
controller. The coupled altitude increase due to aileron deflection needed to execute the 
heading change was about 0.2 feet in Figure III.28 compared to that of around 1 foot in 
the classical controller aircraft in Figure III.25. 





Figure III.28 LQR Controller - Response to Heading Change of +0.2 rad 

In comparison, the LQR controller uses all four control inputs to execute 
commanded changes instead of one control input for each response characteristics as in 
the classical inner-outer loop design. Hence, it is more responsive and more efficient in 
control inputs utilization. For example, comparing Figure 111.28 and Figure 111.25, the 
aileron and rudder inputs to execute heading change is more than one order smaller in the 
case of the integral LQR. However, the integral LQR structure is based on full-state 
feedback and assumes all the states are measurable and not too noisy. Also, the cross- 
coupled nature of the control inputs makes it more difficult to deduce the response of 
each control input in a given flight situation and makes troubleshooting more difficult. 
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IV. NAVIGATION ALGORITHM DESIGN 


This section discusses the development of an infrared vision-based shipboard 
navigation algorithm to determine the position and orientation of an aircraft with respect 
to a ship with three visible points of known separation. It covers the problem formulation 
and includes a simulation example based on the numerical analysis technique proposed in 
[11] to determine the range of an aircraft which is instrumented with an IR camera with 
respect to the ship. The simulation serves as a precursor to explore implementation of an 
autonomous shipboard landing algorithm for the FROG UAV. 

A. SHIPBOARD LANDING PROBLEM FORMULATION 

Design of the autonomous shipboard landing algorithm requires first determining 
the range and orientation of the aircraft to a ship which has a minimum of three 
identifiable points. However, using only three reference points (RPs) always results in 
more than one solution as has been shown by a number of researchers of this problem. 
This non-uniqueness is usually resolved at close ranges by using more than three points 
but for the purposes of this study, it was assumed that three reliable points may be 
computed from the location of the smokestack and the extents (width and height) of the 
ship even when the aircraft is sufficiently far from the ship as an input condition to the 
navigation algorithm development. Figure IV. 1 shows an example of such a scenario that 
can be used by the algorithm developed. 



Figure IV. I Examples showing images of three RPs 

57 






Figure IV.2 The 3-point geometry applied to shipboard navigation 


Onee the image of the three RPs is established, the geometry of the Posed 3-Point 
(P3P) problem to determine the range and orientation of the aireraft’s IR eamera with 
respeet to the ship in the shipboard autolanding navigation seeneario is shown in Figure 
IV.3 formulates the problem formulation as follows; Let p. = {x^,y^,z.], i = l,...,3 
denote the veetors eonneeting the origin of the eamera frame O with the three kn own 
points P., i = 1,...,3 . Let d., i = 1,...,3 denote distanees between these points. Then, 


\P\ ~P2\-^\ ^ 0 5 \p\ ^P-i\-^2 ^ 0 5 \P2 ^P'i\ = ^^3 ^ 0 , d2^ d^. 


(IV. 1) 


and s. =||j5, ||, i = 1,...,3 denote the norms of the veetors p.. Using the pinhole 

eamera model, the projeetion of eaeh RP onto the image plane of the eamera with the 
foeal length/has the following form; 


n 


fe)= 


(u^ 



vLj 

X, 



, i = 1,...,3. 


(IV.2) 
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Figure IV. 3 Three-point perspeetive pose estimation problem geometry 


Combining equations (IV. 1) and (IV.2) result in nine equations with nine 
unknowns {x,., y,., z,.}, i = 1,...,3 . Using (IV.2), 


T,- = 


x,u, _ x,v, 
-. =- 


/ 


/ 


(IV.3) 


Substituting these expressions into the (IV. 1), equations (IV.2) and (IV. 1) can be 
reduced to a set of three nonlinear equations in three unknowns; 


/—I 2 


-|-MjM2 +V^V2')x^X2 ={fd^Y ^ 




+MjM 3 +VjV 3 )rjX 3 ={fd2y , 

(IV.4) 


^f-A 

+W 2 W 3 +V2V2)x2X2 ={fd2)\ 



/= 2,3 


To simplify the notation, (IV.4) can be re-written as follows 
Axl - 2Z)j2XjX2 + Bxl = (ij, 

Axy - 2Z)j3XjX3 + CX 3 = ^2 5 (IV.5) 

Bxl - 2D2^X2X^ + Cxi ~ 


where the coefficients A, B, C, , i = 1,...,3 are strictly positive by construction. 
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Using (IV.5) one can obtain another system of equations better suited for further 
analysis. First, observe that 


/ 

VI' 


J_ 

4b 


^2 ’ 


/ 

V^' 


(IV. 6 ) 


Rewriting system (IV.5) in terms of 5 ., i = I,...,3 gives 

si - 2S]S2 cos«j +sl =dl, 

cos«2+■^3 = (IV.7) 

si - 252^3 COS «3 + ^3 = dl , 


where costtj = 


(pt.Pi) 

lAiiftir 


cosa2 


IaIIIII3 


cos (23 


(from Figure IV.3) 

^2 h 


The system of equations in (IV.7) has an upper bound of eight (2’^2’‘2) real 
solutions. Moreover they form four symmetric pairs, because if a triplet (^j*, 52 ,^ 3 ) is a 

solution, than the triplet - 52 ,- 53 ) forms a solution as well. Geometrically, (IV.7) 

can be described as an intersection of three orthogonal elliptic cylinders with the 
semiaxes rotated around corresponding symmetry axes by the angle of 45°. This follows 
directly from the canonical form of equation (IV.7). The magnitudes of the semiaxes for 
each cylinder are equal to 


a.,b. = 


d^ 


Vi 


±C0S(2; 


/ = !,...,3. 


(IV. 8 ) 


It is clear that the intersection of any two cylinders is always non-empty and the 
number of solutions in this case is infinite. However, by adding a third cylinder one can 
get only a finite number of intersection points. In practice for the system (IV.7), this 
number cannot be zero or two. Therefore, the only possible set of solutions contains four, 
six or eight points. 
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If Assumption (Al) is made such that camera is always in front of the plane 
defined by three RPs Pi, / = 3 , the x-component of each vector p. = 

i = 1,...,3 that satisfies this assumption must be positive (i.e. 5 . > 0, / = 1,...,3 ). 

With the substitution in (IV.9) and (IV. 10), the system of equations in (IV.7) can 
be reduced to a fourth-order polynomial in 5 , . 

u* =— and v’ = — (IV.9) 

5i 

$2 =u* +s^ cos«j and 53 =v* + 5 j cos« 2 r (IV. 10) 

The numerical analysis technique in [11] further showed that with assumptions 
A 2 ) min s I » max ; 

1 = 1,3 1=1,3 

A 3 ) the camera is sufficiently far from the ship. 

3 

0<«. <;r/2, '^a.KTT, = 1,...,3 . (IV.11) 

1=1 z=u 

i*i 


And from first two equations in (IV.7), $2 and 53 can be expressed as: 

± ^(cos«,._i5i)^ -dl ^), i = 2,3 . 


5, = COS«,^j5j 


(IV. 12) 


With the consequence that all possible solutions for 5jthat satisfies assumptions A1-A3 is 
bounded by the interval: 


0 < < 5 * = mm<j 


d: 




-cos a. 


. 1 ^ 


y = mm< 

!=i ,2 sm a. 


(IV. 13) 
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B, 


AIRCRAFT-SHIP ORIENTATION DETERMINATION 


Based on the analysis presented in [11], the following algorithm was applied to 
solve the PSP. Suppose a good initial guess of the normal to the plane generated by 
the three points is available, then for step k: 

i) Solve numerieally equation (IV. 10) for Xj*' in the interval equation (IV. 13), using 
xf*as an initial guess; 


S 2 =u* +5) cos«j and = v* + cos «2 ? 


0 < < 5* = min] 

/= 1,2 


d, 




-cos «, 


d.. 


^ = mm]-— 

'= 1.2 sm«, 

v * 


(IV. 10) 
(IV. 13) 


ii) substitute each solutionx{^^ obtained in i) into (IV.3) to get p)''\ and ^ 


(k) 


\(k) . 


X.u. X.v. 

, z,- = ■ 


/ 


/ 


(IV.3) 


iii) compute normals 


-(k) 

n\ = 


ik) _ ^(k) 
F2 \ 


Pl\ 


P['\ - Pf \ 


iv) choose set p \''\, i = 1,3 or p \’'\, i = 1,3 that maximizes the dot product 



,n 


(k-\) 


Using the solution provided by the PSP algorithm the relative orientation of the 
aircraft with respect to the plane formed by the three RP’s can be computed as follows: 

Let {3p} denotes an orthogonal coordinate system attached to the plane generated 
by the three RP’s, let {c} denotes the coordinate system attached to the camera and let 
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be the eoordinate transformation from {3p} to {c}. Form three orthogonal veetors Fj, 
Fj, F 3 using the eorreet solution p^, p^, p^ as follows: 


{Pi -Px) 


{P2-Pi)^{Pi -Px) 


P 2 - Px 


P2-Px 


Pi -Px 


,r^=r^xr,. 


(IV. 14) 


Then 3 ;i? = [Fi F,]. 


But from geometry, the transformation matrix can also be expressed using Euler 


angles as: 


3p 


R = 


cosy/^pCos0jp 

cosi//,p sin d,psm(p,p-smi//,p cos 

cos(//'3pSin03pCOS(^3p +sm(//'3pSm03p 


sm(//'3pCost?3p 

sin(;/'3pSmt?3pSin^3p + cos^3p C0S(^3p 

sinsink's;, cos^3p -coS5«^3p sin^jp 


cost?3pSm(^3p ’ 

cos6 '3 ^cos^3p_ 


(IV.15) 


where yz-^^ , 6^^, (j)^^ are yaw, piteh and bank angles, respeetively, with respeet to 

the plane formed by the three RP’s. Therefore, the Euler angles ean be found in the 
following manner: 

r, j Tj-, 

y/^p =aretan —, 6^p =-aresinrj 3 , ^ 2 ^ 3 ^ = aretan—. (IV. 16) 


In general, the eoordinate system {3p} does not coineide with the inertial 
eoordinate system {i}. Therefore, the attitude {c} of the eamera with respeet to {i} ean be 
found using (IV.16)from the transformation matrix ^pR^'’R , where ean be obtained 
from the known positions of the three RP’s in {ij, using the same manner. 


C. ALGORITHM SIMULATION 

The PSP algorithm developed above was applied to determine the range of the 
aireraft with respeet to the ship in a simulation example deseribed below.. The ship is 
moving North at a eonstant speed of 10mA. Its motion is eharacterized by piteh and heave 
oseillations with a period of 12sec. The aireraft is performing a left turn with deseent 
from the initial point (-1450, -200, 470)m with respeet to the ship’s initial position at an 
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airspeed of 53m/s. The camera's focal length is / = 0.1m and declination angle with 
respect to a/c longitudinal axis is -6deg. The errors in the projection of each RP onto the 
image plane of the camera are modeled as independent Gaussian random process with 
zero mean and standard deviation of one pixel. 

Figure IV.4 shows the horizontal projection of each of the three RP’s on the ship 
tracked by the camera and of the aircraft’s motion. Figure IV.5 gives the corresponding 
3D representation. 



Figure IV.4 Horizontal projection of a/c’s and ship’s motion 



Figure IV.5 3D representation of the simulation scenario 


In summary, simulation results shows that the proposed algorithm is a feasible 
method of implementing flight navigation and trajectory tracking in order to adopt certain 
flight profiles at various stages of the mission. This can be further demonstrated on the 
FROG UAV test-bed in the near future. 
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V. GROUND TEST RESULTS 


To test capability of the computer in capturing GPS, IMU and PWM data on 
aircraft in the same manner it would operate in flight, a full system verification test was 
conducted at the UAV Lab. All the aircraft equipment and the newly installed miniature 
computer were powered from an internal battery. A floppy was used to boot up the xPC 
operating kernel and then removed for the UAV nose fairing to be installed. The data 
collection application was ‘built’ on the Host PC and uploaded to the aircraft’s computer 
via wireless serial modem. Commands to start and stop data collection were issued from 
the Host PC remotely. The engine was started on several tests to check for 
electromagnetic interference (EMI) on data transfer and to assure pilot control capability 
from various distances. 

A. DATA ANALYSIS 

The ground test proved that the aircraft computer was able to correctly collect and 
interpret the data from various instruments. The GPS, IMU, PWM and A/D data captured 
on a particular data collection test is presented in the next few pages. Figure V.l to V.9 
show the plots of GPS RMC and GGA data captured when the UAV was moved in a 
triangular path of approximately South-West, followed by East, followed by North in 
front of the UAV Eab. 

1. GPS Signals 

From the GPS RMC data captured. Figure V.l shows the correct UTC, while 
Figures V.2 and V.3 show the latitude and longitude of Monterey airport area where the 
test was conducted. The data in Figures V.2 and V.3 was combined and re-presented in 
ground coordinates to show the path taken by the aircraft while data capturing was in 
progress. This is shown in Figure V.4 together with the bearing (215°, followed by 80°, 
followed by 350°) the UAV had taken along the path. Figure V.5 shows the ground speed 
at which the UAV was moved around the path and the date of the test (i.e. 13/03/02). 
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Figure V.6 shows the magnetie variation in degrees and the direetion of magnetic north 
and true north. 


> [ t(35:55),RMC_ 
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Figure V.l GPS RMC UTC (left) and RMC Status (right) 



PM of MfC LawucH CNrcetton 



Figure V.2 GPS RMC Lattitude (left) and Latitude Direction (right) 






Figure V.3 GPS RMC Longitude (left) and Longitude Direction (right) 
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Figure V.4 Position Plot From RMC data (left) and GPS RMC Track (right) 
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Figure V.5 GPS RMC Ground Speed (left) and dd/mm/yy (right) 



Figure V.6 GPS RMC Magnetic Variation (left) and MV Angle (right) 
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The GPS GGA sentence provides data complementary to the GPS RMC sentence 
for navigation purposes. Figures V.7 shows the UTC time in the hh.mm.ss.ss format and 
the number of satellites in use. Figure V.8 demonstrates a clear correspondence to the 
data in Figure V.7 (right) and shows that the horizontal dilution of precision increased to 
around 2.0 when the number of available satellites in sight decreased from 5 to 4, and to 
3.6 when the number of available satellites dropped further to 3. Figure V.9 shows the 
altitude of antenna in meters above mean sea-level. 
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Figure V.7 GPS GGA UTC (left) and Number of Satellite Vehicles Used (right) 
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Figure V.8 GPS GGA Fix Quality (left) and HDOP (right) 
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Figure V.9 GPS GGA Antenna Fleight (left) and DGPS Data Age (right) 


2, A/D and PWM Signals 

The voltages of the control surfaces and throttle servos captured by the A/D signal 
are presented together with the corresponding PWM commands received by the Futaba® 
receiver in Figures V. 10 to V. 13. Each pair of figures clearly shows servos responding to 
the commands issued to it. While the A/D measured signals are almost noise-free, the 
PWM signals captured showed data drop-outs occasionally over a period of 5 to 10 
seconds. As such, some form of filtering needs to be implemented if the PWM captured 
data is required. In our case, this does not pose a serious concern as the PWM signal 
captured is not used for flight control processing. Rather, it is measured to record the 
commands sent to the Futaba® receivers and correlate with how the aircraft responds 
when the Futaba® controller is used. 
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Figure V. 10 Aileron and Elevator Servo Voltages measured by A/D 
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Figure V.l 1 PWM Commands Issued to Aileron and Elevator Servos 
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Figure V. 12 Rudder and Throttle Servo Voltages measured by A/D 




Figure V. 13 PWM Commands Issued to Rudder and Throttle 
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3. 


Crossbow Signals 


The signals from the Crossbow IMU were captured on another ground test after 
Crossbow IMU was initialized. They are presented as time-plots in Figure V.14. The path 
taken as the UAV was pushed on its landing gear around the NFS UAV Lab is given in 
the first plot. The + 2° roll angle measured is in-line with the manner in which the body 
of the aircraft tilts as it moves along its path, except when it reached the slightly sloped 
turnaround point when the left-wing was slightly lower as the aircraft turns. The effect of 
the slight ground slope near the turnaround point was evident in the pitch angle plot. The 
pitch angle increases from 0 to 7° as the aircraft slowly moves up the slope and changes 
to -7° as it turns back. The longitude versus latitude plot shows the aircraft initially 
veered right with respect to its initial heading as it moves away from the UAV Lab. It 
subsequently turned left for some distance before making an about turn. This is clearly 
reflected in the Psi (heading) data measured by Crossbow. 
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The angular rates and linear accelerations recorded by the Crossbow are more 
‘noisy’ but still correlate well with the FROG UAV’s motion. Roll rate and pitch rate 
fluctuate around zero as the UAV moves around since the landing gear introduces 
vibration to the Crossbow. In the yaw-rate data captured, the about-turn at about 70 
seconds correspondent to the left turn the aircraft had to take. The acceleration in the z- 
axis shows +lg due to gravity as expected. 
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B, 


EMI ISSUES 


Despite sueeessful demonstration of sensor data eapture and interpretation on the 
new onboard eomputer, the ground tests revealed two eommunieation related problems in 
the setup. The first was that data downlink from Target PC to Host PC via the Freewave® 
modems was intermittent. The xPC real-time operating system would display a “COM 
failure” message on the Host PC during some downloads. The seeond was that during 
data downloading, the aireraft eontrol surfaces respond to commands sent from the 
Futaba® remote control with significant delay. This is unacceptable as it can result in a 
total loss of control when emergency pilot mode of control is used. Both problems occur 
only when the wireless modem is used in place of a direct null-modem cable between the 
Host PC and Target PC. 

Initial investigations explored the effect of changing some of the modem’s 
transmission parameters such as maximum data packet size, minimum data packet size, 
power setting, the transmission frequency and baud rate but failed to resolve the 
occasional COM failure problem that occurred during data download. Subsequent 
investigations narrowed down to electromagnetic interference (EMI) generated on the 
aircraft and serial communication data latency timings as possible causes. 

The first assessment resulted from the observation that upload of application 
programs and commands to the aircraft computer did not encounter transmission 
problems when radio signals were sent to the onboard modem. However, during the 
downloading of recorded flight data, when the onboard modem was transmitting, EMI 
from cables onboard the aircraft can corrupt the transmitted data and result in 
transmission errors. This hypothesis was supported by absence of communication failure 
when the PWM capture cable was removed. 

Another possible cause could be the delay time the xPC operating kernel expects 
data to be available on the serial link during transmission. This may introduce 
communication failure in the xPC software depending on how much latency time the xPC 
operating kernel would accommodate. In a direct cable link, such a problem does not 
arise as the cable is always connected and does not require transmission handshaking as 
in the wireless modems. 
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The freezing of eontrol surfaee response on the aireraft when the onboard modem 
is transmitting eould be due to the interferenee of the modem transmission on the Futaba® 
reeeiver. 


74 



VI. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

In support of the research objective on coordination and control of a cluster of 
UAVs, a miniature computer critical for onboard real-time data processing in order to 
execute autonomous guidance and control of aircraft was constructed and installed on the 
FROG UAV in this project. In contrast to the original setup where flight and navigation 
data had to be transmitted to a computer on the ground for processing and control 
commands computation before being re-transmitted back to the aircraft for control 
execution, the new onboard computer integrates all the sensor data for control and 
guidance commands computation on the aircraft. This reduces sensor data processing 
time from 25 milliseconds (at 40 Hz downlink data rate) by 2 orders of magnitude to 
around 350 microseconds. It would also reduce control commands execution time to tens 
of microseconds given the computer generates PWM signals to control the servos directly 
as compared to 160 milliseconds in the current setup. The presence of an onboard 
computer also provides the computational capability to implement data processing for 
more advanced research activities in the future. To date, the computer and all software 
drivers written to interface with various onboard instruments have been tested. 

Additionally, a new 6-DOF model of the FROG UAV has been developed in 
Simulink. This will facilitate future simulation of the control and guidance algorithm for 
the FROG UAV. As part of this thesis, two possible autopilot designs - one using 
classical control techniques and one using modem control theory - have also been 
designed for the UAV. A vision-based navigation algorithm for the FROG UAV has also 
been demonstrated in software simulation. These control and navigation algorithms can 
be hardware-in-the-loop tested using available facilities at NFS before verification flights 
in the near future. 

The use of the newly marketed xPC rapid prototyping system in this project 

greatly expedited the development and implementation of the desired control setup. A 

thorough understanding of the capabilities and ways to overcome its shortcomings has 

resulted from this project. The expertise accumulated will be useful for many projects at 
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NPS, not just in the Aeronautics Department, since the rapid prototyping approach can be 
applied to shorten the design-to-full-system testing timeframe of any conceivable system 
being developed. 


B, RECOMMENDATIONS 

With the software for navigation and flight data processing on the aircraft fully 
verified, the only remaining obstacle preventing a flight test pertains to the wireless 
communication EMI issue. The following can be pursued in the near future. 

1. Measure the signal-to-noise level and the signal power level during the uplink and the 
downlink to quantify the signal power required and the noise limits the control setup 
can accommodate. 

2. Determine the components causing EMI and introduce EMI filters accordingly to 
remove or reduce the EMI on the aircraft. 

3. Consult Mathworks, Inc on the serial communication latency time the xPC operating 
kernel can accept and find a way to change this parameter in the xPC operating kernel 
to accommodate any delay in the serial modem transmission. 

4. Explore the use of a wireless Ethernet link in avoiding the interference on the 
downlink transmission. A wireless Ethernet link can also reduce massive flight data 
download times by 1 to 2 orders of magnitude. 
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APPENDIX A 


DESCRIPTION OF FROG UAV 


196 UAVs:USA 

BAITERN 


Type 

Multipurpose, semi-expendable UAV. 

Development 

The TERN {Tactical Expendable Remote 
Navigator) was originally designed and 
developed by H*Cubed Corporation of 
Columbia, Maryland. The programme was 
acquired by BA) Aerosystems in 1993. 
Capable of a variety of applications, it is 
recoverable in peacetime training missions 
but of sufficiently low cost that it could be 
discarded after a battlefield mission if 
necessary. On 1 October 1992, a TERN set 
up an endurance record in FAI Class F3a 
for unmanned aircraft of 33 hours 39 
minutes 15 seconds. A straight-line 
distance record in the same class was set 
on 28 September 1993 with a flight of 
245.80 n miles (455.23 km; 282.87 
miles). 

Airframe 

Pod-and-boom fuselage with high- 
mounted wing, sweptback fin and rudder, 
and low-set tailplane; wing fitted with flaps; 
GFRP/epoxy construction. Engine 
mounted above wing centre-section: fixed 
tricycle landing gear. 

Mission payloads 

Can include colour TV camera, thermal 
imager or hazardous agent sensors. One 
variant (TERN-C) has carried a remote IR 
sensor for aerial detection of chemical 
warfare agents. Another (FOG-R) has been 
fitted with an Optelecom Fibre Optic 
DataLink (FODL) and flown non-line of 
sight 'over and below the hill' for uplink 
command of the UAV and downlink video 
imagery. The latter version is stated to be 
proof against jamming. 

Guidance and control 

UHF (400 MHz) radio command uplink for 
remote control; D-band (1.8 GHz) video 
downlink with data sideband. Options 
include programmable autopilot, GPS 
navigation and fibre optic datalink. Wing 
flaps can be trimmed to enable slow flight 
speeds for surveillance, or fully extended to 
assist landing in restricted spaces. 

Transportation 

Wings detach for containerised storage 
and transportation. 

Launch 

Conventional wheeled take-off. 

Recovery 

Conventional wheeled landing. 

Operational status 

In production. 

Customers 

Has been used by US Army as surrogate 
fibre optic guided missile (FOG-M) and as 
an NBC sensor vehicle. Also used by Naval 
Weapons Center and ARDEC. 

Prime contractor 

BAI Aerosystems Inc, Easton, Maryland. 



Power plant 

One 7.5 kW (10 hp) 150 cc two-cylinder two-stroke engine (type not known); two- 
blade fixed-pitch wooden propeller. 


Dimensions 

Wing span 
Wing area 
Length overall 
Height overall 
Wheel track 


3.10m{10ft 2.0 in) 
1.57 mM 16.94 sq ft) 
2.48m(8ft 1.5 in) 
0.86 m (2 ft 10.0 in) 
0.74 m (2 ft 5.0 in) 


Weights 

Weight empty 
Max payload 
MaxT'O weight 


17.7 kg (39.0 lb) 
13.6 kg (30.0 lb) 
34.0 kg (75.0 lb) 


Performance 

Max level speed 
Normal cruising speed 
Loiter speed 
Stalling speed 

* Range 

Typical endurance at above cruising 
speed 

* Extendable in autonomous flight mode 


70 kt(129 km/h; SOmph) 

48-52 kt (88-96 km/h; 55-60 mph) 
35-43 kt (64-80 km/h; 40-50 mph) 
31 kt(57 km/h; 35 mph) 

8.6 n miles (16 km; 10 miles) 

3 h 


April 1999 


JUAVT-ISSUE 11 
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APPENDIX B. AIRBORNE COMPUTER I/O INTERFACE 


The hardware setting for the miniature eomputer and various I/O boards that was 
built for the UAV is given in the seetions below. Similar information from the original 
AC-104 eomputer is also given in ease the miniature eomputer needs to be re-eonfigured 
baek to the AC-104 eonfiguration. 

1. I/O ADDRESS 

The I/O addresses of resourees in the new miniature eomputer are shown in Table 
B.l. The I/O resouree addresses for the original AC-104 is also shown as referenee. 


AC-104 Computer 

Base Address ‘jj 

(Offset) i|l 

New Computer 

IDE hard disk interface 

OxlFO 

Not Installed 

Reserved 

0xlF8 

Not Installed 

Ruby-MM 

8 Channel D/A, 24 Chan. Dig. I/O 

0x200 (0x40) 

Removed 

AIM/16-1 16 

Channel A/D, 16 Chan. Dig. I/O 

0x280 (0x20) 

AIM/16-1 

Serial Port Number 4 (ET Models Only) 

0x2E8 

COM4 

Reserved 

0x2F0 

Not Installed 

Serial Port Number 2 

0x2F8 

COM2 

Flex/104 IP Carrier Board 1 

0x300 

Removed 

Pause and Error Light Control Register 

0x310 

Available. Not Used 

Flex/104 IP Carrier Board 2 

0x320 

Not Installed 

M-Systems 4MB Flash Disk 

0x330 

4MB Flash Disk 

Ruby-MM-16 

16-channel D/A (optional) 

0x340 

Not Installed 

NAI-5718 Synchro/Resolver Board 

0x355 

Not Installed 

Flex/104 IP Carrier Board 3 

0x360 

Not Installed 

Multimode Parallel Port (when enabled) 

0x378 

Available. Not Used 

Ethernet 

0x380 

Ethernet 

Monochrome display adapter 

0x3B0 

Not Installed 

Flat Panel/CRT VGA display adapter 

0x3C0 

VGA Display 

Video Controller 

0x3D0 

Video Controller 

Serial Port Number 3 (ET models only) 

0x3E8 

COM3 

Reserved 

0x3F0 

Not Installed 

Floppy disk controller ports 

0x3F2 

Floppy disk controller 

Serial Port Number 1 

0x3F8 

0x08 


0x210 

QMM-10 Timer/Counter 
(for PWM capture) 


0x240 

QMM-5 Timer Counter 
(for PWM generation) 


Table B.l I/O Resouree Addresses 
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2 . 


I/O BOARD PIN OUT CONNECTION AND USAGE 


The pin utilization for the I/O eards installed in the miniature computer is given in 
the Tables B.2, B.3 and B.4 below. Bold items indicated connected lines. 


AC104 

Signal 

Board 

Board 

Signal 

AC104 

1 

In 1 

1 

2 

In 2 

26 

2 

Gate 1 

3 

4 

Gate 2 

27 

3 

Out 1 ( PWMl out) 

5 

6 

Out 2 (PWM2 out) 

28 

4 

In 3 

7 

8 

In 4 

29 

5 

Gate 3 

9 

10 

Gate 4 

30 

6 

Out 3 (PWM3 out) 

11 

12 

Out 4 (PWM4 out) 

31 

7 

In 5 

13 

14 

Out 5 

32 

8 

Gate 5 

15 

16 

Frequency Output 

33 

9 

NC 

17 

18 

NC 

34 

10 

NC 

19 

20 

NC 

35 

11 

NC 

21 

22 

NC 

36 

12 

NC 

23 

24 

NC 

37 

13 

NC 

25 

26 

NC 

38 

14 

NC 

27 

28 

NC 

39 

15 

NC 

29 

30 

NC 

40 

16 

NC 

31 

32 

Interrupt Input 

41 

17 

Digital Out 7 

33 

34 

Digital In 7 

42 

18 

Digital Out 6 

35 

36 

Digital In 6 

43 

19 

Digital Out 5 

37 

38 

Digital In 5 

44 

20 

Digital Out 4 

39 

40 

Digital In 4 

45 

21 

Digital Out 3 

41 

42 

Digital In 3 

26 

22 

Digital Out 2 

43 

44 

Digital In 2 

27 

23 

Digital Out 1 

45 

46 

Digital In 1 

28 

24 

Digital Out 0 

47 

48 

Digital In 0 

29 

25 

+5V 

49 

50 

Ground 

50 


Table B.2 QMM-5 (PWM Generation) Pin Interface 


80 
























































































































































AC104 Signal Board Board Signal 

AC104 

1 

In 1 

1 

2 

In 2 

26 

2 

Gate 1 (PWMl In) 

3 

4 

Gate 2 (PWMl In) 

27 

3 

Out 1 

5 

6 

Out 2 

28 

4 

In 3 

7 

8 

In 4 

29 

5 

Gate 3 (PWM2 In) 

9 

10 

Gate 4 (PWM2 In) 

30 

6 

Out 3 

11 

12 

Out 4 

31 

7 

In 5 

13 

14 

Out 5 

32 

8 

Gate 5 

15 

16 

Frequeney Output 

33 

9 

In 6 

17 

18 

In 7 

34 

10 

Gate 6 (PWM3 In) 

19 

20 

Gate 7 (PWM3 In) 

35 

11 

Out 6 

21 

22 

Out 7 

36 

12 

In 8 

23 

24 

In 9 

37 

13 

Gate 8 (PWM4 In) 

25 

26 

Gate 9 (PWM4 In) 

38 

14 

Out 8 

27 

28 

Out 9 

39 

15 

In 10 

29 

30 

Out 10 

40 

16 

Gate 10 

31 

32 

Interrupt Input 

41 

17 

Digital Out 7 

33 

34 

Digital In 7 

42 

18 

Digital Out 6 

35 

36 

Digital In 6 

43 

19 

Digital Out 5 

37 

38 

Digital In 5 

44 

20 

Digital Out 4 

39 

40 

Digital In 4 

45 

21 

Digital Out 3 

41 

42 

Digital In 3 

26 

22 

Digital Out 2 

43 

44 

Digital In 2 

27 

23 

Digital Out 1 

45 

46 

Digital In 1 

28 

24 

Digital Out 0 

47 

48 

Digital In 0 

29 

25 

+5V 

49 

50 

Ground 

50 


Table B.3 QMM-10 (PWM Capture) Pin Interfaee 
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AC104J 

Signal 

Board J 

Board 

Signal 

AC104J 

1 

Analog Gronnd 

1 

2 

V reference (5V) 

26 

2 

Analog In Ch 1 Hi 
(Servo Pot 1 Hi) 

3 

4 

Analog In Ch 1 Lo 
(Servo Pot 1 Lo) 

27 

3 

Analog In Ch 2 Hi 
(Servo Pot 2 Hi) 

5 

6 

Analog In Ch 2 Lo 
(Servo Pot 2 Lo) 

28 

4 

Analog In Ch 3 Hi 
(Servo Pot 3 Hi) 

7 

8 

Analog In Ch 3 Lo 
(Servo Pot 3 Lo) 

29 

5 

Analog In Ch 4 Hi 
(Servo Pot 4 Hi) 

9 

10 

Analog In Ch 4 Lo 
(Servo Pot 4 Lo) 

30 

6 

Analog In Ch 5 Hi 

11 

12 

Analog In Ch 5 Lo 

31 

7 

Analog In Ch 6 Hi 

13 

14 

Analog In Ch 6 Lo 

32 

8 

Analog In Ch 7 Hi 

15 

16 

Analog In Ch 7 Lo 

33 

9 

Analog In Ch 8 Hi 

17 

18 

Analog In Ch 8 Lo 

34 

10 

Analog Gronnd 

19 

20 

+15V 

35 

11 

-15V 

21 

22 

Digital I/O Ground 

36 

12 

Digital I/O Ch 1 

23 

24 

Digital I/O Ch 2 

37 

13 

Digital I/O Ch 3 

25 

26 

Digital I/O Ch 4 

38 

14 

Digital I/O Ch 5 

27 

28 

Digital I/O Ch 6 

39 

15 

Digital I/O Ch 7 

29 

30 

Digital I/O Ch 8 

40 

16 

Digital I/O Ch 9 

31 

32 

Digital I/O Ch 10 

41 

17 

Digital I/O Ch 11 

33 

34 

Digital I/O Ch 12 

42 

18 

Digital I/O Ch 13 

35 

36 

Digital I/O Ch 14 

43 

19 

Digital I/O Ch 15 

37 

38 

Digital I/O Ch 16 

44 

20 

External Trigger 

39 

40 

Digital I/O Ground 

45 


NOTE: Channels 21-25 and 46-50 are not eonneeted on the AC-104 front panel 


Table B.4 AIM16 (Servo Pots & Differential Pressure Sensor) Pin Interfaee 
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3. 


INTERRUPT ROUTINE (IRQ) ASSIGNMENT 


The interrupt routine veetor addresses for the new miniature eomputer is given in 
Table B.5. The IRQ assignment for the original AC-104 eomputer is also shown as 
referenee should the miniature eomputer needs to be re-eonfigured baek to the original 
AC-104 eonfiguration. 


IRQ 

AC-104 I/O Resource 

New I/O Resource J 

IRQO 

ROM BIOS clock tick function 

Same 

IRQl 

Keyboard 

Same 

IRQ2 

Cascaded inputs from IRQs 8-15 

QMM-5 

IRQ3 

Serial Ports 2 (and 4, for ET models only) 

COM2 

IRQ4 

Serial Ports 1 (and 3, for ET models only) 

COMl 

IRQ5 

LPT2 or Flex/104 IP Carrier Board 3 

COM4 

IRQ6 

Floppy drive controller 

Same 

IRQ? 

Parallel Port or optional AX-10425 
frequency driver 

QMM-10 

IRQ8 

Watchdog timer and error handling 

Same 

IRQ9 

Ethernet 

Same 

IRQ 10 

Controller pause function 

COM3 

IRQll 

Flex/104 IP Carrier Board 1 

Removed 

IRQ 12 

Not Used 

Same 

IRQ 13 

Reserved for co-processor (not used) 

Same 

IRQ 14 

IDE hard disk controller 

Same 

IRQ 15 

Flex/104 IP Carrier Board 2 

Same 


Table B.5 IRQ Assignment 
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APPENDIX C. SOFTWARE DRIVERS & LQR DESIGN CODE 


This appendix contains the “C” source code written to receive and decode the data 
streams from the Agl32 GPS receiver and the Crossbow AHRS400CA-100 serving as 
the IMU. The IMU and GPS interface drivers were written by the author for this project 
while the GPS messages decoding routines were written by Dr. Vladimir Dobrokhodov 
for a previous project. All the C-codes had to be packaged into MATLAB’s S-Function 
Level 2 structure which adopts a specific sequence to initialize a simulation block, update 
its states, control sampling rates, output data and terminate the function. Each set of code 
has to be “mex” by a compatible C-compiler in MATLAB and ‘build’ into executable 
code by xPC’s Real-Time Workshop before it can be called from within a Simulink block 
as a S-Function. Details of how this is done are discussed in Refs [12] and [13]. 


1. CROSSBOW AHRS DATA RECEIVE DRIVER 


/* - */ 

/* CROSSBOW AHRS Interface Driver */ 

/* This routine is a modification of the rs232brec.c routine from Mathworks. */ 

/* Function: Receive undelimited RS232 bytes from Crossbow AHRS. */ 

/* Implements a two-buffer system to collect all available data */ 

/* Search for Crossbow header byte, test if checksum of next X-1 bytes */ 

/* tally with checksum byte. If so, valid Crossbow message. Output. */ 

/* If not adequate bytes to form new message, exit with last message */ 

/* X = width of Crossbow message specified by user in block's mask */ 

/* Jan 18, 2002 */ 

/* Filename: xbowrcvlp3.c */ 

/* Written by: Bock-Aeng Lim */ 

/* - */ 

/* Original Source file comments: 

/* $Revision: 1.1 $ $Date: 2001/07/20 22:11:41 $ */ 


/* rs232rec.c - xPC Target, non-inlined S-function driver for RS-232 receive (asyn) */ 
/* Copyright 1996-2001 The MathWorks, Inc. 

*/ 

#define S_FUNCTION_LEVEL 2 

#undef S_FUNCTION_NAME 

#define S_FUNCTION_NAME xbowrcvlp3 

#include <stddef.h> 

#include <stdlib.h> 

#include "tmwtypes.h" 

#include "simstruc.h" 

#ifdef MATLAB_MEX_FILE 
#include "mex.h" 

#else 

#include <windows.h> 

#include <string.h> 

#include "rs232_xpcimport.h" 

#include "time_xpcimport.h" 
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#endif 


/* Input Arguments from Simulink block's user mask */ 


#define NUMBER_OF_ARGS 
#define PORT_ARG 
#define WIDTH_ARG 
#define SAMP TIME ARG 


(3) 

ssGetSFcnParam(S,0) /* 
ssGetSFcnParam(S,I) /* 
ssGetSFcnParam(S,2) /* 


COM port to use */ 
max width */ 

User specified sample time */ 


#define NO_I_WORKS 
bufCount */ 

#define NO_R_WORKS 
#define NO_P_WORKS 
#define NO D WORKS 


(3) /* current pos pointer in buf, rec length, 

( 0 ) 

( 0 ) 

(1) /* for buf array */ 


#define HEADER 


255 


/* Crossbow message header byte */ 


static char_T msg[256]; 
extern int rs232ports[]; 


static void mdllnitializeSizes(SimStruct *S) 

{ 

#ifndef MATLAB_MEX_FILE 
#include "rs232_xpcimport.c" 

#include "time_xpcimport.c" 

#endif 

ssSetNumSFcnParams(S, NUMBER_OF_ARGS); 

if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 

sprintf(msg,"Wrong number of input arguments passed.\n" 
"%d arguments are expectedXn",NUMBER_OF_ARGS); 
ssSetErrorStatus(S,msg); 
return; 

} 


/* Set-up size information */ 
ssSetNumContStates( S, 0); 
ssSetNumDiscStates( S, 0); 
ssSetNumOutputPorts(S, 2); 
ssSetNumInputPorts( S, 2); 


/* data, "done pulse" */ 
/* rec length, enable */ 


ssSetOutputPortWidth(S, 0, 1); 


/* Function-call */ 


ssSetOutputPortWidth(S, 1, (int)mxGetPr(WIDTH_ARG) [0] ) ; 

ssSetOutputPortDataType(S, 1, SS_UINT8); 


ssSetInputPortDirectFeedThrough(S, 0, 1); 
ssSetInputPortDirectFeedThrough(S, 1, 1); 
ssSetInputPortWidth ( S, 0, 1); 

ssSetInputPortWidth( S, 1, 1); 

ssSetInputPortRequiredContiguous(S, 0, 1); 
ssSetInputPortRequiredContiguous(S, 1, 1); 

ssSetNumSampleTimes(S,1); 
ssSetNumlWork(S, NO_I_WORKS); 
ssSetNumRWork(S, NO_R_WORKS); 
ssSetNumPWork(S, NO_P_WORKS); 
ssSetNumDWork(S, NO_D_WORKS); 

ssSetDWorkDataType(S, 0, SS_UINT8); 

ssSetDWorkWidth( S, 0, (int)mxGetPr(WIDTH_ARG)[0]); 

ssSetDWorkWidth ( S, 0, 2048); 


ssSetNumModes( S, 0); 

ssSetNumNonsampledZCs( S, 0); 


ssSetSFcnParamNotTunable (S,0) ; 
ssSetSFcnParamNotTunable (S,1) ; 
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ssSetSFcnParamNotTunable(S,2); 


ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_PLACE_ASAP); 

} 

/* Function to initialize sample times */ 
static void mdllnitializeSampleTimes(SimStruct *S) 

{ 

ssSetSampleTime(S, 0, mxGetPr(SAMP_TIME_ARG)[0]); 
if (mxGetN((SAMP_TIME_ARG))==1) { 

ssSetOffsetTime(S, 0, 0.0); 

} else { 

ssSetOffsetTime(S, 0, mxGetPr(SAMP_TIME_ARG)[1]); 

} 

ssSetCallSystemOutput(S, 0); 

} 

#define MDL_START /* Change to #undef to remove function */ 

#if defined(MDL_START) 

static void mdlStart(SimStruct *S) 

{ 

#ifndef MATLAB_MEX_FILE 

ssGetIWork(S)[0] = 0; /* set current buf pointer = 0 */ 

ssGetIWork(S)[2] = 0; /* set bufCount = 0 */ 

#endif 

} 

#endif 


/* Function to compute outputs */ 

static void mdlOutputs(SimStruct *S, int_T tid) 

{ 

#ifndef MATLAB_MEX_FILE 

int width = (int)mxGetPr(WIDTH_ARG) [0]; /* specify output port width */ 

int port = (int)mxGetPr(PORT_ARG )[0] - 1; /* specify COM# */ 
unsigned char tmp; /* temp char holder */ 

unsigned char *buf = (unsigned char *)ssGetDWork(S, 0); /* uchar buffer to contain 

bytes from serial port*/ 

int *current = ssGetIWork(S);/* current = addr of current position pointer in buf */ 
int *recLength = ssGetIWork(S) + I; /* recLength = addr of receieved data length */ 
int *bufCount = ssGetIWork(S)+ 2; /* count number of useful bytes in buf. */ 

int serbufCount; /* count number of useful bytes collected in Serial buf*/ 

int HeaderFound, i, j, bufStop, sumbytes, chksum; 
int nextbytetoprocess, lastHeaderPos, EOB; 


if (ssGetInputPortRealSignal(S, 1)[0] == 0) 
return; 


/* function is disabled. Stop 
processing and get out */ 


serbufCount = rl32eReceiveBufferCount(port); /* Check number of bytes available */ 

while (serbufCount) { /* transfer everything in serial buffer to buf */ 

tmp = rl32eReceiveChar(port); 

if ((tmp & OxffOO) != 0) { /* only last 8 bits can be non-zero */ 

printf("RS232Receive Error; char & OxffOO != 0 \n"); 
return; 


buf[(*current)++] = tmp & Oxff; 
serbufCount--; 

(*bufCount)++; 


/* put valid char into buf */ 

/* reduce serbufCount */ 

/* increase bufCount correspondingly */ 


// FROM HERE, IMPLEMEMT CHECKSUM COMPUTATION & TRANSFER DECODED MESSAGE TO OUTPUT 
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HeaderFound = 0; 
i = 0; 

EOB = 0; 


if (*bufCount<30) return; // Not enough bytes to decode, output old value 

while (HeaderFound==0) { 

/* find Header byte */ 

while (buf[i] != HEADER) { 

if (i < *bufCount) i++; 

else {EOB = 1; 
break;} 

} /* at exit, buf[i] = HEADER or EOB =1 */ 


if (EOB == 1) { 

nextbytetoprocess = lastHeaderPos; 
break; 


if (*bufCount - i < width-1) { 
nextbytetoprocess = i; 
break; 

} 

else { 

sumbytes =0; /* Compute checksum */ 

for (j=1;j<width-l;j++) { 

sumbytes = sumbytes + buf[i+j]; 

} 

chksum = sumbytes % 256; 

if (chksum == buf[i+width-1]) { 

HeaderFound = 1; 

memcpy(ssGetOutputPortSignal(S,1),buf+i,width); 
nextbytetoprocess = i + width; 

} // if checksum tally 
else { // checksum doesn't tally 
lastHeaderPos = i; 

i++; //skip current FF, find next FF 

} 

} //else 

} /* while HeaderFound = 0*/ 


if (*bufCount>100) { //BufCount large, too many old bytes, flush buffer 

*bufCount = 0; 

*current = *bufCount; 


else { // Pack useful bytes in buf to front of buf for next routine call 

bufStop = *bufCount; // bufStop = no. of bytes in pre-packed buf 
*bufCount = *bufCount - nextbytetoprocess; // bufCount = no. of 

bytes after packing 

1 = 0 ; 

for (j=nextbytetoprocess;jkbufStop;j++) { 

buf [1] = buf [j] ; 
i++; 

} 

*current = *bufCount; // update pointer to end of buf 

} 

ssCallSystemWithTid(S, 0, 0); /* issue done pulse to outport 0 */ 


return; 


#endif 

} 

/* Function to perform housekeeping at execution termination */ 
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static void mdlTerminate(SimStruct *S) 

{ 

} 


#ifdef MATLAB_MEX_FILE 
#include "simulink.c" 
#else 

#include "cg_sfun.h" 
#endif 


/* Is this file being compiled as a MEX-file? */ 
/* MEX-file interface mechanism */ 

/* Code generation registration function */ 


2, GPS DATA RECEIVE DRIVER 


/* - */ 

/* TRIMBLE GPS Interface Driver */ 

/* This routine is a modification of the rs232brec.c routine from Mathworks. */ 

/* Function; Receive undelimited RS232 bytes from Agl32 GPS. Search for sentence */ 
/* header Compute checksum for next 5 bytes to determine if sentence*/ 

/* is 'GPGGA' or 'GPRMC'. Form message, output message at Portl in */ 

/* X byte width, output Header Index at Port2 to identify 'GPGGA' */ 

/* or 'GPRMC for follow-on processing. */ 

/* X = is max width of message specified by user in block's mask */ 

/* Original Feb 1, 2002 */ 

/* Revised Mar 11, 2002 */ 

/* Filename: gpsrcv.c */ 

/* Written by: Bock-Aeng Lim and Dr. Vladimir Dobohodkov */ 

/* - */ 

/* Original comments: 

/* SRevision; 1.1 $ $Date: 2001/07/20 22:11:41 $ */ 

/* rs232rec.c - xPC Target, non-inlined S-function driver for RS-232 receive */ 


#define S_FUNCTION_LEVEL 2 
#undef S_FUNCTION_NAME 
#define S_FUNCTION_NAME gpsrcv 

#include <stddef.h> 

#include <stdlib.h> 


#include "tmwtypes.h" 
#include "simstruc.h" 

#ifdef MATLAB_MEX_FILE 
#include "mex.h" 

#else 

#include <windows.h> 

#include <string.h> 

#include "rs232_xpcimport.h" 
#include "time_xpcimport.h" 
#endif 


/* Input Arguments */ 
#define NUMBER_OF_ARGS 
#define PORT_ARG 
#define WIDTH_ARG 
sentence */ 

#define SAMP_TIME_ARG 

#define NO_I_WORKS 
#define NO_R_WORKS 
#define NO_P_WORKS 
#define NO_D_WORKS 

#define HEADER 

static char_T msg[256]; 
extern int rs232ports[ ] ; 


(3) 

ssGetSFcnParam(S,0) 

ssGetSFcnParam(S,1) /* max width is the max length of GPS 

ssGetSFcnParam(S,2) 

(3) /* current pos ptr in buf, rec length, bufCount*/ 

( 0 ) 

( 0 ) 

(1) /* for buf array */ 

(36) // $- sign 


// unsigned char* gl_buf; global variable to save captured bytes between sample steps 
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static void mdllnitializeSizes(SimStruct *S) 

{ 

#ifndef MATLAB_MEX_FILE 
#include "rs232_xpcimport.c" 

#include "time_xpcimport.c" 

#endif 

ssSetNumSFcnParams(S, NUMBER_OF_ARGS); 

if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 

sprintf(msg,"Wrong number of input arguments passed.\n" 
"%d arguments are expectedXn",NUMBER_OF_ARGS); 
ssSetErrorStatus(S,msg); 
return; 

} 


/* Set-up size information */ 
ssSetNumContStates( S, 0); 
ssSetNumDiscStates( S, 0); 
ssSetNumOutputPorts(S, 3) ; 
ssSetNumInputPorts( S, 2); 

ssSetOutputPortWidth(S, 0, 1); 

ssSetOutputPortWidth(S, 1, 
ssSetOutputPortDataType(S, 


/* fuc-call,data, header ind */ 
/* rec length, enable */ 

/* Function-call */ 


(int)mxGetPr(WIDTH_ARG)[0]);/* Data */ 
1, SS_UINT8); 


ssSetOutputPortWidth(S, 2, 1); /* Header index */ 

SsSetOutputPortDataType(S, 2, SS_UINT8); 


ssSetInputPortDirectFeedThrough(S, 0, 1); 
ssSetInputPortDirectFeedThrough(S, 1, 1); 
ssSetInputPortWidth( S, 0, 1); 

ssSetInputPortWidth( S, 1, 1); 

ssSetInputPortRequiredContiguous(S, 0, 1); 
ssSetInputPortRequiredContiguous(S, 1, 1); 

ssSetNumSampleTimes(S,1); 
ssSetNumlWork(S, NO_I_WORKS); 
ssSetNumRWork(S, NO_R_WORKS); 
ssSetNumPWork(S, NO_P_WORKS); 
ssSetNumDWork(S, NO_D_WORKS); 

ssSetDWorkDataType(S, 0, SS_UINT8); 

ssSetDWorkWidth( S, 0, (int)mxGetPr(WIDTH_ARG)[0]); 

ssSetDWorkWidth( S, 0, 2048); 


ssSetNumModes( S, 0); 

ssSetNumNonsampledZCs( S, 0); 

ssSetSFcnParamNotTunable(S, 0) ; 
ssSetSFcnParamNotTunable(S, 1) ; 
ssSetSFcnParamNotTunable(S, 2) ; 

ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_PLACE_ASAP) 


/* Function to initialize sample times */ 
static void mdllnitializeSampleTimes(SimStruct *S) 

{ 

ssSetSampleTime(S, 0, mxGetPr(SAMP_TIME_ARG)[0]); 
if (mxGetN((SAMP_TIME_ARG))==1) { 

ssSetOffsetTime(S, 0, 0.0); 

} else { 

ssSetOffsetTime(S, 0, mxGetPr(SAMP_TIME_ARG)[1]); 

} 

ssSetCallSystemOutput(S, 0); 
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#define MDL_START /* Change to #undef to remove function */ 

#if defined(MDL_START) 

static void mdlStart(SimStruct *S) 

{ 

#ifndef MATLAB_MEX_FILE 

ssGetIWork(S)[0] = 0; /* set current buf pointer = 0 */ 

ssGetIWork (S) [2] = 0; /* set bufCount = 0 */ 


#endif 

} 

#endif 


/* Function to compute outputs */ 
static void mdlOutputs(SimStruct *S, int_T tid) 
{ 

#ifndef MATLAB MEX FILE 


int width = 

(int)mxGetPr(WIDTH 

ARG) 

[0] ; 

/* specify output 

port width 

=WIDTH ARG that is 

the max length of 

GPS 

sentence 

*/ 


int port 

(int)mxGetPr(PORT ARG ) 

[0] - 1; 

/* specify COM# */ 


unsigned char 

tmp; 



/* 

temp char holder 


*/ 

unsigned char *buf = (unsigned char *)ssGetDWork (S, 0); /* uchar buffer to contain 
bytes from serial port*/ 

int *current = ssGetIWork(S); /* current = addr of 

current position pointer in buf */ 

int *recLength = ssGetIWork(S) + 1; /* recLength = addr of 

receieved data length */ 

int *bufCount = ssGetIWork(S)+ 2; /* count number of useful 

bytes in buf. */ 

int serbufCount; /* count number of 

useful bytes collected in Serial buf*/ 

int HeaderFound, i, j, bufStop, chksum, nextbytetoprocess, lastHeaderPos, EOB; 
int GGA=358,RMC=377;// checksum of 'GPGGA','GPRMC' sentences's header 
int* bl_header;// boolean values for GGA=1 and RMC=2 sentences, 0=nothing found 
int headwidth=5;// length of GPS header except '$' 


if (ssGetInputPortRealSignal(S, 1)[0] == 0) 
processing and get out */ 
return; 


/* function is disabled. Stop 


*/ 


serbufCount = rl32eReceiveBufferCount(port); /* Check number of bytes available 


while (serbufCount) 

{ /* transfer everything in serial buffer to buf */ 

tmp = rl32eReceiveChar(port); 
if ((tmp & OxffOO) != 0) 

{ /* only last 8 bits can be non-zero */ 


printf("RS232Receive Error; char & 
return; 

} 

buf[(*current)++] = tmp & Oxff; 
serbufCount--; 

(*bufCount)++; 

bufCount correspondingly */ 

} 

/*Initialize logical flags*/ 
HeaderFound = 0; 
i = 0; 

EOB = 0;//end of buffer 


xffOO != 0 \n") ; 


/* put valid char into buf */ 
/* reduce serbufCount */ 

/* increase 


if (*bufCount<width) return; // Not enough bytes to decode, output old value 


while (HeaderFound==0) 

{/* find Header byte = '$'=36 */ 
while (buf[i] != HEADER) 

{ 


if (i < *bufCount) 


i++; 
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else 

{ 

EOB = 1; break; 

} 

} /* at exit, buf[i] = HEADER or EOB =1 */ 
if (EOB == 1) 

{ nextbytetoprocess = lastHeaderPos; 
break; 

}//end of if 

if (*bufCount - i < width-1) 

{ nextbytetoprocess = i; 
break; 


else 

{chksum =0; /* Compute checksum */ 

for (j = 1;j<headwidth+l;j++) 

{ 

chksum = chksum + buf[i+j]; 

} 

}// calculate "checksum" of GPS header={$GPGGA,$GPRMC} 
if (chksum == GGA) 

{ HeaderFound = 1; 

*bl_header=l; 
ggalng=0; 

while(*(buf+i+headwidth+1+ggalng) != 13) ggalng++; //"0D"=13, 
memcpy(ssGetOutputPortSignal(S,1),buf+i+headwidth+l,ggalng); 
nextbytetoprocess = i + ggalng; 

} // if checksum tally 
else 


if (chksum == RMC) 

{ HeaderFound = 1; 

*bl_header=2 ; 
rmclng = 0; 

while(*(buf+i+headwidth+l+rmclng) != 13) rmclng+l; 
memcpy(ssGetOutputPortSignal(S,1),buf+i+headwidth+l,rmclng); 
nextbytetoprocess = 1 + rmclng; 

} // if checksum tally 
else 

{ // checksum doesn't tally 
lastHeaderPos = 1; 

*bl_header=0; 

}// end of checksum searching 


memcpy(ssGetOutputPortSignal(S,2),bl_header,1); 

} /* while HeaderFound = 0*/ 
if (*bufCount>300) 

{ // If bufCount too large, too many old bytes, just flush buffer 
*bufCount = 0; 

*current = *bufCount; 

} 

else { // Pack useful bytes in buf to front of buf. 

bufStop = *bufCount; // bufStop = no. of bytes in pre-packed buf 
*bufCount = *bufCount - nextbytetoprocess; // bufCount = no. of 

bytes after packing 

1 = 0 ; 

for (j=nextbytetoprocess;jkbufStop;j++) 

{ 

buf [1] = buf [j] ; 
i++; 

} 

*current = *bufCount; // update pointer to end of buf 

} 


ssCallSystemWithTid(S, 0, 0); /* issue done pulse to outport 0 */ 
return; 
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#endif 


} 

/* Function to perform housekeeping at execution termination */ 
static void mdlTerminate(SimStruct *S) 

{ 

} 


#ifdef MATLAB_MEX_FILE 
#include "simulink.c" 
#else 

#include "cg_sfun.h" 
#endif 


/* Is this file being compiled as a MEX-file? */ 
/* MEX-file interface mechanism */ 

/* Code generation registration function */ 


3. GPS GPGGA MESSAGE DECODER 


/* - */ 

/* GPGGA MESSAGE DECODING ROUTINE */ 

/* Function; This routine decodes the output from gpsrcv block and decode the */ 

/* GPGGA message. */ 

/* Feb 1, 2002 */ 

/* Filename: gpgga.c */ 

/* Written by: Vladimir Dobohodkov */ 

/* - */ 

/* 

* File : gpgga.c 

* $Revision; 1.00 $V.Dobrokhodov 
*/ 


#include <stdlib.h> 

#include <math.h> 

#include <stdio.h> 

#include <iostream.h> 

#include <string.h> 

#define S_FUNCTION_NAME gpgga 
#define S_FUNCTION_LEVEL 2 

#include "simstruc.h" 

/* Input Arguments */ 

#define NUMBER_OF_ARGS (1) 

#define WIDTH ssGetSFcnParam(S,0) /* WIDTH is the max length of incoming 

GPS sentence */ 

j*================* 

* Build checking * 

*================* j 

static char_T msg[256]; 


/* Function; mdllnitializeSizes =========================================; 

* Abstract; 

* Setup sizes of the various vectors. 

*/ 

static void mdllnitializeSizes(SimStruct *S) 

{ 

ssSetNumSFcnParams(S, NUMBER_OF_ARGS); 
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 

sprintf(msg,"Wrong number of input arguments passed.\n" 

"%d arguments are expectedXn",NUMBER_OF_ARGS); 
ssSetErrorStatus(S,msg); 

return; /* Parameter mismatch will be reported by Simulink */ 

} 

if (!ssSetNumInputPorts(S, 1)) return; 

ssSetInputPortWidth(S, 0, (int)mxGetPr(WIDTH)[0]);//DYNAMICALLY_SIZED 
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ssSetInputPortDirectFeedThrough(S, 0, 1) ; 


if (!ssSetNumOutputPorts(S,1)) return; 
ssSetOutputPortWidth(S, 0, 14);//14DYNAMICALLY_SIZED 


ssSetNumSampleTimes(S, 1); 


} 


/* Take care when specifying exception free code - see sfuntmpl_doc.c */ 
SSSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | 

SS_OPTION_USE_TLC_WITH_ACCELERATOR); 


/* Function: mdllnitializeSampleTimes ================================ 

* Abstract: 

* Specifiy that we inherit our sample time from the driving block. 
*/ 

static void mdllnitializeSampleTimes(SimStruct *S) 

{ 

SSSetSampleTime(S, 0, INHERITED_SAMPLE_TIME); 
ssSetOffsetTime(S, 0, 0,0); 

} 


int gpssmbl(int_T* input,int length) 

{ 


char tmp[]="\0",output[]="\0"; 

int i,gpstmp; 

for (i=0;i<length;i++) 

{ tmp[0]=(char)(*input);strncat(output,tmp,1);input++;} 

switch (output[0]) 

gpstmp=0;break;/*North*/ 
gpstmp=l;break;/*East*/ 
gpstmp=2;break;/*South*/ 
gpstmp=3;break;/*West*/ 
gpstmp=4;break;/*Valid or Autonomous*/ 
gpstmp=5;break;/*Differential*/ 
gpstmp=6;break;/*Non-Valid*/ 
gpstmp=7;break;/*Meters*/ 
gpstmp=6;break;/*Non-Valid*/ 


};// 


{ case 

case 
case 
case 
case 
case 
case 
case 

default: 
}/*end of switch*/ 
return gpstmp; 
end of gpssmbl 


'N' : 

'E' 

'S' 

' W' 
'A' 

' D' 

' V' 
'M' 


/* Function: bin2ascii =====================================================: 

Abstract: function provides GPS data convertion to ASCII and then to FLOAT 
representations */ 

void bin2ascii(int_T* in,int length, char* ext) 

{ int* input=in; 

char tmp[]="\0"; 
int i; 

for (i=0;i<length;i++) 

{ 

tmp[0]=(char)(*input); 
strncat(ext,tmp,1); 
input++; 

} 

};// end of bin2ascii 


/* Function: mdlOutputs ======================================================= 

* Abstract: 

* 

*/ 

static void mdlOutputs(SimStruct *S, int_T tid) 

{ 

int_T i=0,j=0; 

InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);/* Incoming data stream*/ 
real_T *y = ssGetOutputPortRealSignal(S,0); 

/*int_T width = ssGetOutputPortWidth(S,0);*/ 
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int_T tempbuf[100]; //here we make an aliase for the uPtrs 
char ext[]="\0"; 
int count=0,len_in; 

double sys[20];//output array of decoded data 
double res[l]={0}; //output piece of decoded data 
real_T tmp; 

for (i=0; i<(*uPtrs[0]); i++){tempbuf[i] = ( int_T)(*uPtrs[i+1]);} 


//GPGGA sentence of GPS message 
i=0; 

if ((real_T) tempbuf[i] != (real_T) 44) 

{ /*printf("\n Error! 44 expected, received %d",*tempbuf);*/ 
return;} 

else count++; /*miss first comma sign and define shift*/ 

/* UTC & Latitude -1,2*/ 
for(i=0;i<2;i++) 

{ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext) ; 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 
}/*end of for*/ 


/*Direction of latitude -3*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gpssmbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 
/*Longitude -4*/ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 

/*Direction of longitude -5*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gpssmbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 

/*GPS quality indicator -6; 

Number of SVs -7; 

HOOP -8; 

Antenna height -9*/ 

for(i=0;i<4;i++) 

{ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext) ; 
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*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 
}/* end of for*/ 


/*Altitude in meters -10*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gpssmbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 


/*Geoidal separation -11*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext) ; 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 


/*Geoidal separation in meters -12*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gpssmbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 


/*Age of DGPS data -13*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext) ; 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 


/*Base station ID-14*/ 
len_in=0;/*initialize it again*/ 

while (tempbuf[count+len_in] != 42)/*42 ='*' It's the beginning of next gps 
message*/ 

{++len_in;}//end of while to count the length of GPS field - looking for next 
","{44-ASCII} 

bin2ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof (ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 
/*END of DGPS GGA sentence*/ 

/****************************************************************************/ 

} 


/* Function: mdlTerminate ============================================ 

* Abstract: 

* No termination needed, but we are required to have this routine. 
*/ 

static void mdlTerminate(SimStruct *S) 

{ 

} 


#ifdef MATLAB MEX FILE 

/* 

#include 

"simulink.c" 

/* 

#else 



#include 

"eg sfun.h" 

/* 

#endif 




Is this file being compiled as a MEX-file? 
MEX-file interface mechanism */ 

Code generation registration function */ 


*/ 


96 





4, 


GPS GPRMC MESSAGE DECODER 


/* - */ 

/* GPRMC MESSAGE DECODING ROUTINE */ 

/* Function; This routine decodes the output from gpsrcv block and decode the */ 

/* GPRMC message. */ 

/* Feb 1, 2002 */ 

/* Filename: gprmc.c */ 

/* Written by: Vladimir Dobohodkov */ 

/* - */ 

/* 

* File : gprmc.c 

* SRevision; 1.00 $V. Dobrokhodov 
*/ 


#include <stdlib.h> 
#include <math.h> 
#include <stdio.h> 
#include <iostream.h> 
#include <string.h> 


#define S_FUNCTION_NAME gprmc 
#define S_FUNCTION_LEVEL 2 

#include "simstruc.h" 

/* Input Arguments */ 

#define NUMBER_OF_ARGS (1) 

#define WIDTH ssGetSFcnParam(S,0) /* WIDTH is the max length of incoming 

GPS sentence */ 

j*================* 

* Build checking * 

*================* j 

static char_T msg[256]; 

/* Function; mdllnitializeSizes =============================================== 

* Abstract: 

* Setup sizes of the various vectors. 

*/ 

static void mdllnitializeSizes(SimStruct *S) 

{ 


ssSetNumSFcnParams(S, NUMBER_OF_ARGS); 
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 

sprintf(msg,"Wrong number of input arguments passed.\n" 

"%d arguments are expectedXn",NUMBER_OF_ARGS); 
ssSetErrorStatus(S,msg); 

return; /* Parameter mismatch will be reported by Simulink */ 

} 


if (!ssSetNumInputPorts(S, 1)) return; 
ssSetInputPortWidth(S, 0, (int)mxGetPr(WIDTH)[0]);//DYNAMICALLY_SIZED 

ssSetInputPortDirectFeedThrough(S, 0, 1); 

if (!ssSetNumOutputPorts(S,1)) return; 
ssSetOutputPortWidth(S, 0,12 );//12=RMC 


ssSetNumSampleTimes(S, 1); 


/* Take care 
ssSetOptions 


when specifying exception free code - 
(S, SS_OPTION_EXCEPTION_FREE_CODE | 
SS_OPTION_USE_TLC_WITH_ACCELERATOR); 


see sfuntmpl_doc.c */ 


/* Function; mdllnitializeSampleTimes 
* Abstract: 
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* Specifiy that we inherit our sample time from the driving block. 
*/ 

static void mdllnitializeSampleTimes(SimStruct *S) 

{ 

SSSetSampleTime(S, 0, INHERITED_SAMPLE_TIME); 
ssSetOffsetTime(S, 0, 0.0); 

} 

/* Function: gps_smbl ================================================ 

* Abstract: function provides GPS symbol informatin processing */ 


int gps_smbl(int_T* input,int length) 


char tmp[]="\0",output[]="\0"; 

int i,gpstmp; 

for (i=0;i<length;i++) 

{ tmp[0]=(char)(*input);strncat(output,tmp,1);input++;} 

switch (output[0]) 


{ 


};// 


case 
case 
case 
case 
case 
case 
case 
case 
default: 
}/*end of switch*/ 
return gpstmp; 
end of gps_smbl 


'N' 
'E' 
'S' 
' W' 
'A' 
' D' 
' V' 
'M' 


gpstmp=0;break;/*North*/ 
gpstmp=l;break;/*East*/ 
gpstmp=2;break;/*South*/ 
gpstmp=3;break;/*West*/ 
gpstmp=4;break;/*Valid or Autonomous*/ 
gpstmp=5;break;/*Differential*/ 
gpstmp=6;break;/*Non-Valid*/ 
gpstmp=7;break;/*Meters*/ 
gpstmp=6;break;/*Non-Valid*/ 


/* Function: bin2_ascii ====================================================: 

Abstract: function provides GPS data convertion to ASCII and then to FLOAT 
representations */ 

void bin2_ascii(int_T* in,int length, char* ext) 

{ int* input=in; 

char tmp[]="\0"; 
int i; 

for (i=0;i<length;i++) 

{ 

tmp[0]=(char)(*input); 
strncat(ext,tmp,1); 
input++; 

} 

};// end of bin2_ascii 


/* Function: mdlOutputs ======================================================= 

* Abstract: 

* 

*/ 

static void mdlOutputs(SimStruct *S, int_T tid) 

{ 

int_T i=0,j=0; 

InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);/* Incoming data stream*/ 
real_T *y = ssGetOutputPortRealSignal(S,0); 

/*int_T width = ssGetOutputPortWidth(S,0);*/ 

/***********************jnterface for C++ programm**********************************/ 
int_T tempbuf[100]; //here we make an aliase for the uPtrs 
char ext[]="\0"; 
int count=0,len_in; 

double sys[20];//output array of decoded data 
double res[l]={0}; //output piece of decoded data 
real_T tmp; 

/**********************]yierge data to temporary array***************************/ 
for (i=0; i<(*uPtrs[0]); i++)(tempbuf[i] = ( int_T)(*uPtrs[i+1]);} 
/****************************************************************************/ 


//GPRMC sentence of GPS message 
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i=0; 

if ((real_T) tempbuf[i] != (real_T) 44) return; 

else count++; /*miss first comma sign and define shift*/ 

/* UTC -1*/ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2_ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 


/*Status -2*/ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gps_smbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 
/*Latitude -3*/ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2_ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 

/*Latitude direction -4*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gps_smbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 
/*Longitude -5*/ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2_ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 

/*Direction of logitude -6*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gps_smbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 

/*Speed over ground[knots] -7*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2_ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext) ; 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 

/*Track made good,True[degree] -8*/ 
len_in=0;/*initialize it again*/ 
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while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2_ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 

/*Date in dd/mm/yy -9; 

Manetic variation [degree] -10;*/ 

for(i=0;i<2;i++) 

{ 

len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

bin2_ascii((tempbuf+count),len_in,ext); 

*y++=(real_T)atof(ext); 

*ext=NULL;/*initialize it again*/ 

count+=len_in+l;//miss next comma sign and define new shift 
}/* end of for*/ 

/*Direction of Magnetic variation-11*/ 
len_in=0;/*initialize it again*/ 
while (tempbuf[count+len_in] != 44) 

{++len_in;}//end of while to count the length of GPS field - looking for next 
,"{44-ASCII} 

*y++=gps_smbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 


/*Mode indicator(A(4)-autonomous;D(5)-differencial;N(0)-not valid) -12*/ 

len_in=0;/*initialize it again*/ 

while (tempbuf[count+len_in] != 42)/*=42*/ 

{++len_in;}//end of while to count the length of GPS field - looking for next 
","{44-ASCII} 

*y++=gps_smbl((tempbuf+count),len_in); 

count+=len_in+l;//miss next comma sign and define new shift 
/*END of DGPS RMC sentence*/ 

/****************************************************************************/ 

] 


/* Function; mdlTerminate ============================================ 

* Abstract: 

* No termination needed, but we are required to have this routine. 
*/ 

static void mdlTerminate(SimStruct *S) 

{ 

} 


#ifdef MATLAB_MEX_FILE 
#include "simulink.c" 
#else 

#include "cg_sfun.h" 
#endif 


/* Is this file being compiled as a MEX-file? */ 
/* MEX-file interface mechanism */ 

/* Code generation registration function */ 
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5, 


MATLAB CODE FOR LQR CONTROLLER DESIGN 


close all; 
clear all; 
clc; 

load frogabcdlO; % load plant model 

1 = 1 ; 

rank(ctrb(as,bs)) % check that (A,B) is controllable and 

rank(obsv(as,cs'*cs)) % (A,Q) is observable. Criteria for LQR. Done only 

once. 

%% Design parameters for tzeros in synthesis.mdl 

xi_h = 0.9; 

wn_h = 0.4; 

xi_v = 0.9; 

wn V = 1.2; 

pole_vt = 0.7; 

pole_psi = 1.5; 

% Design parameters for LQR 

ql = diag([l, 1, 1, 1]); % increase qii, to increase cmd loop bw 

rl = diag([2000,5000,1500,2]); % decrease rii, to increase control loop bw 

%% Obtaining the synthesis model 
disp ( ' ' ) ; 

disp('tranmission zeros of plant with Vt,h,v,psi output'); 
tzero(as,bs,cs,ds) 

[asl,bsl,csl,dsl] = linmod('synthesis'); %,'v5'); 
disp('transmssion zeros of synthesis model'); 
damp(tzero(asl,bsl,csl,dsl) ) 

Synth = [1 2*xi_h*wn_h wn_h^2]; 
syntv = [1 2*xi v*wn v wn v^2]; 
disp('Synthesis zeros are:') 
damp(roots(synth)); 
damp(roots(syntv)); 
disp(-pole_vt); 
disp(-pole_psi); 

%% Computing the feedback gains 
[k,p,e] = Iqr(asl,bsl,csl'*ql*csl, r1); 


%% Obtaining the closed-loop system 
kp = k(;,l:10); 
ki = k(;,11:14) ; 

[ac,be,cc,dc] = linmod('modellO'); %,'v5'); 


%% Check criteria 1: Feedback system must be stable 
disp('Ensure that all the closed loop poles are stable'); 
damp(eig(ac)) 

%% Now include actuator into model to see time-response 
[acl,bcl,ccl,del] = linmod('modellOwAct'); %,'v5'); 
damp(eig(acl)) 

% plots Vt_out to step input in Vt_cmd 
figure(1); 1=1+1; 

step (acl,bcl (;,5) ,ccl (5, ;) ,dcl (5,5) ) ; 

% axis([0 20 -0.2 1.8] ) 

title('Vt out to step input in Vt cmd'); 

% plots h_out to step input in h_cmd 
figure(1); 1=1+1; 

step( acl,bcl(;,6),ccl(6,;),del(6,6) ); 
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% axis ( [0 20 -0.2 1.8]) 

title('h out to step input in h cmd'); 

grid; 

% plots v_out to step input in v_cmd 
figure(i); i=i+l; 

step] acl,bcl(;,7),ccl(7,;),del(7,7) ); 

% axis([0 20 -0.2 1.8]) 

title('v out to step input in v cmd'); 

grid; 

% plots psi_out to step input in psi_cmd 
figure(i); i=i+l; 

step] acl,bcl ];,8),ccl]8,;),del]8,8) ); 

% axis][0 20 -0.2 1.8]) 

title ]'psi out to step input in psi cmd'); 
grid; 


%% Criteria 5: Aileron, Elevator, Rudder loop bandwidth < 10 rad/s 
%% Thrust loop bandwidth < 5 rad/s 

figure ]i); i=i+l; 

margin] acl,bcl];,1),ccl]1,;),del ] 1,1) ) 

title]'Bode of closed loop from del aileron to aileron out. ]BW ~ 10 rad/s) '); 
figure]!); i=i+l; 

margin] acl,bcl];,2),ccl]2,;),del ]2,2) ) 

title]'Bode of closed loop from del elevator to elevator out. ]BW ~ 10 rad/s)') 
figure]!); i=i+l; 

margin] acl,bcl];,3),ccl]3,;),del ]3,3) ) 

title]'Bode of closed loop from del rudder to rudder out. ]BW ~ 10 rad/s) '); 
figure]!); i=i+l; 

margin] acl,bcl];,4),ccl ]4,;),del ]4,4) ) 

title]'Bode of closed loop from del thrust to thrust out. ]BW ~ 5 rad/s) '); 


%% Criteria 4: Gain margin in elevator and thrust loops should 
%% be at least 6 db and phase margin 45 degrees. 

[aol,bol,col,dol] = linmod]'open_a'); 
figure]!); i=i+l; 

margin]aol,bol];,1),col ] 1,;),dol ] 1,1)) 

title]'Bode of open loop from del aileron cmd to aileron out'); 
disp]'Gain and phase margin for aileron loop'); 

[ao2,bo2,co2,do2] = linmod]'open_e'); 
figure]!); i=i+l; 

margin]ao2,bo2];,2),co2 [ 2 ,:) ,do2 [ 2 , 2 )) 

title]'Bode of open loop from del elevator cmd to elevator out'); 
disp]'Gain and phase margin for elevator loop'); 


[ao3,bo3,co3,do3] = linmod]'open_r'); 
figure]!); i=i+l; 

margin]ao3,bo3];,3),co3 ] 3,;),do3 ] 3,3)) 

title]'Bode of open loop from del rudder cmd to rudder out'); 
disp]'Gain and phase margin for rudder loop'); 


[ao4,bo4,co4,do4] = linmod]'open_th'); 
figure]!); i=i+l; 

margin]ao4,bo4];,4),co4 ] 4,;),do4 ] 4,4)) 

title]'Bode of open loop from del thrust cmd to thrust out'); 
disp]'Gain and phase margin for thrust loop'); 

break; 
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